Improve OptiTrack reconnect definition refresh
This commit is contained in:
parent
c55103a3e5
commit
407c20470e
@ -16,6 +16,7 @@ public class OptitrackStreamingClientEditor : Editor
|
||||
private VisualElement runtimeOffline;
|
||||
private VisualElement runtimeOnline;
|
||||
private VisualElement runtimeInfo;
|
||||
private Label runtimeActionText;
|
||||
|
||||
public override VisualElement CreateInspectorGUI()
|
||||
{
|
||||
@ -36,10 +37,11 @@ public class OptitrackStreamingClientEditor : Editor
|
||||
runtimeOffline = root.Q("runtimeOffline");
|
||||
runtimeOnline = root.Q("runtimeOnline");
|
||||
runtimeInfo = root.Q("runtimeInfo");
|
||||
runtimeActionText = root.Q<Label>("runtimeActionText");
|
||||
|
||||
var reconnectBtn = root.Q<Button>("reconnectBtn");
|
||||
if (reconnectBtn != null)
|
||||
reconnectBtn.clicked += () => { if (Application.isPlaying) client.Reconnect(); };
|
||||
reconnectBtn.clicked += () => RunPlayModeAction("Full reconnect requested", () => client.Reconnect());
|
||||
|
||||
root.schedule.Execute(UpdatePlayModeState).Every(300);
|
||||
|
||||
@ -83,9 +85,21 @@ public class OptitrackStreamingClientEditor : Editor
|
||||
SetStatusStyle(null);
|
||||
if (statusText != null)
|
||||
statusText.text = "Stopped";
|
||||
if (runtimeActionText != null)
|
||||
runtimeActionText.text = "";
|
||||
}
|
||||
}
|
||||
|
||||
private void RunPlayModeAction(string message, System.Action action)
|
||||
{
|
||||
if (!Application.isPlaying || client == null)
|
||||
return;
|
||||
|
||||
action?.Invoke();
|
||||
if (runtimeActionText != null)
|
||||
runtimeActionText.text = message;
|
||||
}
|
||||
|
||||
private void SetStatusStyle(bool? connected)
|
||||
{
|
||||
if (statusDot == null || statusText == null) return;
|
||||
|
||||
@ -95,6 +95,13 @@
|
||||
background-color: #b45309;
|
||||
}
|
||||
|
||||
.opti-runtime-action {
|
||||
font-size: 11px;
|
||||
color: #fbbf24;
|
||||
-unity-font-style: bold;
|
||||
margin-bottom: 4px;
|
||||
}
|
||||
|
||||
.opti-runtime-info {
|
||||
padding: 4px 0;
|
||||
}
|
||||
|
||||
@ -41,6 +41,20 @@
|
||||
</ui:Foldout>
|
||||
</ui:VisualElement>
|
||||
|
||||
<!-- Connection Health -->
|
||||
<ui:VisualElement class="section">
|
||||
<ui:Foldout text="Connection Health" value="true" class="section-foldout">
|
||||
<uie:PropertyField binding-path="ConnectionHealthCheckIntervalSeconds" label="Health Check Interval"/>
|
||||
<uie:PropertyField binding-path="StreamingFrameTimeoutSeconds" label="Frame Timeout"/>
|
||||
<uie:PropertyField binding-path="FullReconnectInitialDelaySeconds" label="Full Reconnect Initial Delay"/>
|
||||
<uie:PropertyField binding-path="FullReconnectFrameWaitSeconds" label="Full Reconnect Frame Wait"/>
|
||||
<uie:PropertyField binding-path="FullReconnectRetryDelaySeconds" label="Full Reconnect Retry Delay"/>
|
||||
<uie:PropertyField binding-path="DirectServerInfoTimeoutMs" label="Server Info Timeout Ms"/>
|
||||
<uie:PropertyField binding-path="DirectModelDefTimeoutMs" label="MODELDEF Timeout Ms"/>
|
||||
<ui:HelpBox message-type="Info" text="Reconnect refreshes direct NatNet server info and MODELDEF before rebuilding the local receiver. Lower timeouts make reconnect more responsive when the command channel is unavailable."/>
|
||||
</ui:Foldout>
|
||||
</ui:VisualElement>
|
||||
|
||||
<!-- Definition Refresh -->
|
||||
<ui:VisualElement class="section">
|
||||
<ui:Foldout text="Definition Refresh" value="true" class="section-foldout">
|
||||
@ -72,7 +86,8 @@
|
||||
<ui:HelpBox message-type="Info" text="Runtime controls are available in Play Mode."/>
|
||||
</ui:VisualElement>
|
||||
<ui:VisualElement name="runtimeOnline" class="opti-runtime-online">
|
||||
<ui:Button name="reconnectBtn" text="OptiTrack Reconnect" class="opti-reconnect-btn"/>
|
||||
<ui:Button name="reconnectBtn" text="Full Reconnect" class="opti-reconnect-btn"/>
|
||||
<ui:Label name="runtimeActionText" class="opti-runtime-action"/>
|
||||
<ui:VisualElement name="runtimeInfo" class="opti-runtime-info"/>
|
||||
</ui:VisualElement>
|
||||
</ui:Foldout>
|
||||
|
||||
@ -47,8 +47,8 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
|
||||
public float interpolationDelay = 0f;
|
||||
|
||||
private OptitrackSkeletonDefinition m_skeletonDef;
|
||||
private int m_boundSkeletonDefinitionId = -1;
|
||||
private string previousSkeletonName;
|
||||
|
||||
private OptitrackStreamingClient m_boundStreamingClient;
|
||||
|
||||
[HideInInspector]
|
||||
@ -295,10 +295,19 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
|
||||
RebindStreamingClient();
|
||||
}
|
||||
|
||||
public void RefreshSkeletonDefinitionBinding()
|
||||
{
|
||||
if (StreamingClient == null)
|
||||
InitializeStreamingClient();
|
||||
|
||||
RebindStreamingClient();
|
||||
}
|
||||
|
||||
private void RebindStreamingClient()
|
||||
{
|
||||
m_boundStreamingClient = StreamingClient;
|
||||
m_skeletonDef = null;
|
||||
m_boundSkeletonDefinitionId = -1;
|
||||
m_boneIdToMappingIndex.Clear();
|
||||
m_snapshotPositions.Clear();
|
||||
m_snapshotOrientations.Clear();
|
||||
@ -314,6 +323,7 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
|
||||
m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
|
||||
if (m_skeletonDef != null)
|
||||
{
|
||||
m_boundSkeletonDefinitionId = m_skeletonDef.Id;
|
||||
RebuildBoneIdMapping();
|
||||
isSkeletonFound = true;
|
||||
}
|
||||
@ -595,18 +605,21 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
|
||||
{
|
||||
if (StreamingClient != null)
|
||||
{
|
||||
m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
|
||||
OptitrackSkeletonDefinition latestDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
|
||||
|
||||
if (m_skeletonDef != null)
|
||||
if (latestDef != null)
|
||||
{
|
||||
bool definitionChanged = latestDef.Id != m_boundSkeletonDefinitionId;
|
||||
m_skeletonDef = latestDef;
|
||||
OptitrackSkeletonState skelState = StreamingClient.GetLatestSkeletonState(m_skeletonDef.Id);
|
||||
bool wasFound = isSkeletonFound;
|
||||
isSkeletonFound = (skelState != null);
|
||||
|
||||
if (isSkeletonFound && !wasFound)
|
||||
if (isSkeletonFound && (!wasFound || definitionChanged))
|
||||
{
|
||||
StreamingClient.RegisterSkeleton(this, SkeletonAssetName);
|
||||
previousSkeletonName = SkeletonAssetName;
|
||||
m_boundSkeletonDefinitionId = m_skeletonDef.Id;
|
||||
RebuildBoneIdMapping();
|
||||
Debug.Log($"[OptiTrack] 스켈레톤 '{SkeletonAssetName}' 연결 성공");
|
||||
}
|
||||
@ -622,6 +635,8 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
|
||||
{
|
||||
// 스켈레톤 정의를 찾지 못함 → 지수 백오프로 재조회 요청 빈도 제한
|
||||
isSkeletonFound = false;
|
||||
m_skeletonDef = null;
|
||||
m_boundSkeletonDefinitionId = -1;
|
||||
m_definitionRefreshRequests++;
|
||||
|
||||
// 처음 3회까지는 즉시 요청, 이후 백오프 적용
|
||||
|
||||
@ -123,6 +123,9 @@ public class OptitrackSkeletonDefinition
|
||||
/// <summary>Skeleton asset name.</summary>
|
||||
public string Name;
|
||||
|
||||
/// <summary>True when the definition was inferred from frame packets and does not contain a Motive asset name.</summary>
|
||||
public bool IsSynthetic;
|
||||
|
||||
/// <summary>Bone names, hierarchy, and neutral pose position information.</summary>
|
||||
public List<BoneDefinition> Bones;
|
||||
|
||||
@ -348,6 +351,35 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
[Tooltip("Automatically retries the initial connection and reconnects when streaming frames stop.")]
|
||||
public bool AutoReconnect = true;
|
||||
|
||||
[Header("Connection Health")]
|
||||
[Tooltip("How often connection health is checked. This only reads local timestamps and does not contact Motive.")]
|
||||
[Min(0.1f)]
|
||||
public float ConnectionHealthCheckIntervalSeconds = 0.25f;
|
||||
|
||||
[Tooltip("Seconds without live/replay frames before the stream is considered stale.")]
|
||||
[Min(0.5f)]
|
||||
public float StreamingFrameTimeoutSeconds = 1.0f;
|
||||
|
||||
[Tooltip("Delay before the first full reconnect attempt after a stale stream is detected.")]
|
||||
[Min(0f)]
|
||||
public float FullReconnectInitialDelaySeconds = 0.0f;
|
||||
|
||||
[Tooltip("Seconds to wait for frames after a full reconnect attempt before retrying.")]
|
||||
[Min(0.25f)]
|
||||
public float FullReconnectFrameWaitSeconds = 1.5f;
|
||||
|
||||
[Tooltip("Delay before subsequent full reconnect attempts.")]
|
||||
[Min(0.5f)]
|
||||
public float FullReconnectRetryDelaySeconds = 2.0f;
|
||||
|
||||
[Tooltip("Timeout for the direct NatNet server-info command during connect/reconnect, in milliseconds.")]
|
||||
[Min(100)]
|
||||
public int DirectServerInfoTimeoutMs = 500;
|
||||
|
||||
[Tooltip("Timeout for the direct NatNet MODELDEF command during connect/reconnect, in milliseconds.")]
|
||||
[Min(100)]
|
||||
public int DirectModelDefTimeoutMs = 1000;
|
||||
|
||||
[Tooltip("Changes to the version of Natnet used by the server")]
|
||||
public string ServerNatNetVersion = "";
|
||||
public string ClientNatNetVersion = "";
|
||||
@ -385,6 +417,11 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
private int m_directTimeoutCount = 0;
|
||||
private bool m_directNatNetConnected = false;
|
||||
private bool m_directNoModelDefWarned = false;
|
||||
private IPAddress m_directServerAddress = null;
|
||||
private IPAddress m_directLocalAddress = null;
|
||||
private IPAddress m_directMulticastAddress = null;
|
||||
private UInt16 m_directCommandPort = 0;
|
||||
private UInt16 m_directDataPort = 0;
|
||||
|
||||
private NatNetClient m_client;
|
||||
private NatNetClient m_replayClient;
|
||||
@ -426,6 +463,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
/// <summary>Maps from a streamed skeleton names to its component.</summary>
|
||||
private Dictionary<string, MonoBehaviour> m_skeletons = new Dictionary<string, MonoBehaviour>();
|
||||
private HashSet<MonoBehaviour> m_registeredSkeletonComponents = new HashSet<MonoBehaviour>();
|
||||
|
||||
/// <summary>Maps from a streamed trained markerset names to its component.</summary>
|
||||
private Dictionary<string, MonoBehaviour> m_tmarkersets = new Dictionary<string, MonoBehaviour>(); // trained markerset added
|
||||
@ -445,7 +483,16 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
/// <summary>Cache from streamed asset ID to asset name.</summary>
|
||||
private Dictionary<Int32, string> m_assetIdToNameCache = new Dictionary<Int32, string>();
|
||||
|
||||
private HashSet<Int32> m_latestFrameRigidBodyIds = new HashSet<Int32>();
|
||||
private HashSet<Int32> m_currentFrameRigidBodyIds = new HashSet<Int32>();
|
||||
private HashSet<Int32> m_latestFrameSkeletonIds = new HashSet<Int32>();
|
||||
private HashSet<Int32> m_currentFrameSkeletonIds = new HashSet<Int32>();
|
||||
private bool m_hasFrameRigidBodyTopologySnapshot = false;
|
||||
private bool m_hasFrameSkeletonTopologySnapshot = false;
|
||||
|
||||
private volatile bool m_pendingDefinitionRefresh = false;
|
||||
private volatile bool m_forceDefinitionRefreshSoon = false;
|
||||
private volatile bool m_pendingSkeletonDefinitionNotify = false;
|
||||
private float m_definitionRefreshCooldown = 0f;
|
||||
|
||||
private bool m_isReconnecting = false;
|
||||
@ -473,8 +520,39 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
m_pendingDefinitionRefresh = true;
|
||||
}
|
||||
|
||||
private bool CanRefreshDefinitions()
|
||||
{
|
||||
return !SkipDataDescriptions &&
|
||||
(m_client != null || (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected));
|
||||
}
|
||||
|
||||
private void RefreshDefinitionsForActiveTransport()
|
||||
{
|
||||
if (m_client != null)
|
||||
{
|
||||
UpdateDefinitions();
|
||||
ResubscribeRegisteredAssets();
|
||||
return;
|
||||
}
|
||||
|
||||
if (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected)
|
||||
{
|
||||
UpdateDirectDefinitions();
|
||||
ResubscribeRegisteredAssets();
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("No active OptiTrack transport is available for DataDescription refresh.");
|
||||
}
|
||||
|
||||
private void Update()
|
||||
{
|
||||
if (m_pendingSkeletonDefinitionNotify)
|
||||
{
|
||||
m_pendingSkeletonDefinitionNotify = false;
|
||||
NotifyRegisteredSkeletonDefinitionsChanged();
|
||||
}
|
||||
|
||||
if (m_directFrameLogPending)
|
||||
{
|
||||
m_directFrameLogPending = false;
|
||||
@ -683,14 +761,20 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
m_latestTMarkMarkerSpheres.Clear();
|
||||
}
|
||||
|
||||
if (m_forceDefinitionRefreshSoon)
|
||||
{
|
||||
m_forceDefinitionRefreshSoon = false;
|
||||
m_pendingDefinitionRefresh = true;
|
||||
m_definitionRefreshCooldown = 0f;
|
||||
}
|
||||
|
||||
// Refresh streamed asset definitions when Motive actors or rigid bodies are added/removed.
|
||||
if (m_pendingDefinitionRefresh && m_definitionRefreshCooldown <= 0f && m_client != null && !SkipDataDescriptions)
|
||||
if (m_pendingDefinitionRefresh && m_definitionRefreshCooldown <= 0f && CanRefreshDefinitions())
|
||||
{
|
||||
m_pendingDefinitionRefresh = false;
|
||||
try
|
||||
{
|
||||
UpdateDefinitions();
|
||||
ResubscribeRegisteredAssets();
|
||||
RefreshDefinitionsForActiveTransport();
|
||||
m_definitionRefreshCooldown = Mathf.Max(DefinitionRefreshInterval, 1f);
|
||||
m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
|
||||
}
|
||||
@ -706,8 +790,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
m_definitionRefreshCooldown -= Time.deltaTime;
|
||||
|
||||
if (AutoRefreshDefinitions &&
|
||||
m_client != null &&
|
||||
!SkipDataDescriptions &&
|
||||
CanRefreshDefinitions() &&
|
||||
m_definitionRefreshCooldown <= 0f &&
|
||||
Time.unscaledTime >= m_nextAutoDefinitionRefreshTime)
|
||||
{
|
||||
@ -780,13 +863,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
return;
|
||||
}
|
||||
|
||||
if (m_directNatNetConnected && ConnectionType == ClientConnectionType.Multicast)
|
||||
{
|
||||
Debug.Log(GetType().FullName + ": direct UDP receiver is already active; reconnect request skipped.", this);
|
||||
return;
|
||||
}
|
||||
|
||||
Debug.Log("OptiTrack: reconnect requested.");
|
||||
Debug.Log("OptiTrack: full reconnect requested.");
|
||||
|
||||
OnDisable();
|
||||
|
||||
@ -795,7 +872,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
/// <summary>
|
||||
/// Coroutine for handling the reconnection process with a retry loop.
|
||||
/// Attempts up to 5 times: 1s delay on first attempt, 5s on subsequent.
|
||||
/// Attempts up to 5 times using the configurable full reconnect timing values.
|
||||
/// Last known pose is preserved during reconnect (m_latestSkeletonStates not cleared).
|
||||
/// </summary>
|
||||
private System.Collections.IEnumerator ReconnectCoroutine()
|
||||
@ -806,13 +883,16 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
for (int attempt = 1; attempt <= maxAttempts; attempt++)
|
||||
{
|
||||
m_receivedFrameSinceConnect = false;
|
||||
float delay = (attempt == 1) ? 1.0f : 5.0f;
|
||||
float delay = (attempt == 1)
|
||||
? Mathf.Max(FullReconnectInitialDelaySeconds, 0f)
|
||||
: Mathf.Max(FullReconnectRetryDelaySeconds, 0.5f);
|
||||
Debug.Log(string.Format("{0}: reconnect attempt {1}/{2} in {3} seconds...", GetType().FullName, attempt, maxAttempts, delay), this);
|
||||
if (delay > 0f)
|
||||
yield return new WaitForSeconds(delay);
|
||||
|
||||
yield return StartCoroutine(ConnectCoroutine());
|
||||
|
||||
float deadline = Time.realtimeSinceStartup + 3.0f;
|
||||
float deadline = Time.realtimeSinceStartup + Mathf.Max(FullReconnectFrameWaitSeconds, 0.25f);
|
||||
while (!m_receivedFrameSinceConnect && Time.realtimeSinceStartup < deadline)
|
||||
yield return null;
|
||||
|
||||
@ -1231,6 +1311,9 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
{
|
||||
OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i];
|
||||
|
||||
if (skelDef.IsSynthetic)
|
||||
continue;
|
||||
|
||||
if ( skelDef.Name.Equals( skeletonAssetName, StringComparison.InvariantCultureIgnoreCase ) )
|
||||
{
|
||||
return skelDef;
|
||||
@ -1577,6 +1660,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
}
|
||||
|
||||
PruneStatesMissingFromDefinitions();
|
||||
NotifyRegisteredSkeletonDefinitionsChanged();
|
||||
|
||||
}
|
||||
|
||||
@ -1639,6 +1723,25 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
SubscribeMarkers();
|
||||
}
|
||||
|
||||
private void NotifyRegisteredSkeletonDefinitionsChanged()
|
||||
{
|
||||
if (m_registeredSkeletonComponents.Count == 0)
|
||||
return;
|
||||
|
||||
var registeredSkeletons = new List<MonoBehaviour>(m_registeredSkeletonComponents);
|
||||
foreach (MonoBehaviour component in registeredSkeletons)
|
||||
{
|
||||
if (component == null)
|
||||
{
|
||||
m_registeredSkeletonComponents.Remove(component);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (component is OptitrackSkeletonAnimator_Mingle animator && animator.isActiveAndEnabled)
|
||||
animator.RefreshSkeletonDefinitionBinding();
|
||||
}
|
||||
}
|
||||
|
||||
public void RegisterRigidBody( MonoBehaviour component, Int32 rigidBodyId )
|
||||
{
|
||||
if ( m_rigidBodies.TryGetValue( rigidBodyId, out MonoBehaviour existingComponent ) )
|
||||
@ -1673,6 +1776,8 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
public void RegisterSkeleton(MonoBehaviour component, string name)
|
||||
{
|
||||
m_registeredSkeletonComponents.Add(component);
|
||||
|
||||
if (m_skeletons.TryGetValue(name, out MonoBehaviour existingComponent))
|
||||
{
|
||||
if (existingComponent == component)
|
||||
@ -1899,7 +2004,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
return liveFresh || replayFresh;
|
||||
}
|
||||
|
||||
private void StartDirectFrameReceiver(string localAddress, string multicastAddress, UInt16 dataPort)
|
||||
private bool StartDirectFrameReceiver(string localAddress, string multicastAddress, UInt16 dataPort)
|
||||
{
|
||||
StopDirectFrameReceiver();
|
||||
|
||||
@ -1942,11 +2047,13 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
};
|
||||
m_directFrameThread.Start();
|
||||
Debug.Log(GetType().FullName + ": direct NatNet frame receiver joined " + multicastAddress + ":" + dataPort + " on " + localAddress + (EnableReplayPriority ? " with replay priority group 239.255.42.100." : "."), this);
|
||||
return true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
StopDirectFrameReceiver();
|
||||
Debug.LogWarning(GetType().FullName + ": direct NatNet frame receiver failed to start: " + ex.Message, this);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -2095,26 +2202,63 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
return true;
|
||||
}
|
||||
|
||||
private void QueueStreamedTopologyDefinitionRefresh()
|
||||
{
|
||||
if (!AutoRefreshDefinitions || SkipDataDescriptions)
|
||||
return;
|
||||
|
||||
m_pendingDefinitionRefresh = true;
|
||||
m_forceDefinitionRefreshSoon = true;
|
||||
}
|
||||
|
||||
private static bool UpdateFrameTopologyIds(HashSet<Int32> latestIds, HashSet<Int32> currentIds, ref bool hasSnapshot)
|
||||
{
|
||||
if (!hasSnapshot)
|
||||
{
|
||||
latestIds.Clear();
|
||||
foreach (Int32 id in currentIds)
|
||||
latestIds.Add(id);
|
||||
hasSnapshot = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (latestIds.SetEquals(currentIds))
|
||||
return false;
|
||||
|
||||
latestIds.Clear();
|
||||
foreach (Int32 id in currentIds)
|
||||
latestIds.Add(id);
|
||||
return true;
|
||||
}
|
||||
|
||||
private void ParseDirectRigidBodies(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||||
{
|
||||
int o = start;
|
||||
for (int i = 0; i < count && o < end; i++)
|
||||
m_currentFrameRigidBodyIds.Clear();
|
||||
for (int i = 0; i < count; i++)
|
||||
{
|
||||
sRigidBodyData rbData;
|
||||
if (!ReadDirectRigidBody(data, ref o, end, out rbData)) return;
|
||||
m_currentFrameRigidBodyIds.Add(rbData.Id);
|
||||
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id);
|
||||
RigidBodyDataToState(rbData, frameTimestamp, rbState);
|
||||
}
|
||||
|
||||
if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot))
|
||||
QueueStreamedTopologyDefinitionRefresh();
|
||||
}
|
||||
|
||||
private void ParseDirectSkeletons(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||||
{
|
||||
int o = start;
|
||||
for (int i = 0; i < count && o + 8 <= end; i++)
|
||||
m_currentFrameSkeletonIds.Clear();
|
||||
for (int i = 0; i < count; i++)
|
||||
{
|
||||
if (o + 8 > end) return;
|
||||
int skeletonId = BitConverter.ToInt32(data, o); o += 4;
|
||||
int boneCount = BitConverter.ToInt32(data, o); o += 4;
|
||||
if (boneCount < 0 || boneCount > 4096 || o + boneCount * 38 > end) return;
|
||||
m_currentFrameSkeletonIds.Add(skeletonId);
|
||||
|
||||
sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, boneCount);
|
||||
for (int b = 0; b < boneCount; b++)
|
||||
@ -2124,6 +2268,9 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
CommitDirectSkeletonFrame(skeletonId, stagedBones, boneCount, frameTimestamp);
|
||||
}
|
||||
|
||||
if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot))
|
||||
QueueStreamedTopologyDefinitionRefresh();
|
||||
}
|
||||
|
||||
private void CommitDirectSkeletonFrame(int skeletonId, sRigidBodyData[] stagedBones, int boneCount, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||||
@ -2136,8 +2283,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
if (skelDef == null)
|
||||
{
|
||||
if (!m_pendingDefinitionRefresh)
|
||||
m_pendingDefinitionRefresh = true;
|
||||
QueueStreamedTopologyDefinitionRefresh();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -2208,6 +2354,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
{
|
||||
Id = skeletonId,
|
||||
Name = skeletonName,
|
||||
IsSynthetic = true,
|
||||
Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(boneCount),
|
||||
BoneIdToParentIdMap = new Dictionary<int, int>(),
|
||||
};
|
||||
@ -2237,6 +2384,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
m_skeletonDefinitions.Add(skelDef);
|
||||
m_mirrorBoneIdMaps.Clear();
|
||||
m_pendingSkeletonDefinitionNotify = true;
|
||||
return skelDef;
|
||||
}
|
||||
|
||||
@ -2415,12 +2563,94 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3];
|
||||
ClientNatNetVersion = "Direct UDP";
|
||||
m_directNatNetConnected = true;
|
||||
StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort);
|
||||
if (!StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort))
|
||||
return false;
|
||||
StoreDirectEndpoint(serverAddr, localAddr, commandPort, negotiatedDataPort, negotiatedMulticast);
|
||||
m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
|
||||
Debug.Log(GetType().FullName + ": Connected to direct NatNet server. Server=" + serverAddr + ", host=" + hostName + ", local=" + localAddr + ", serverNatNet=" + ServerNatNetVersion + ", multicast=" + negotiatedMulticast + ":" + negotiatedDataPort + ".", this);
|
||||
return true;
|
||||
}
|
||||
|
||||
private void StoreDirectEndpoint(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, UInt16 dataPort, IPAddress multicastAddr)
|
||||
{
|
||||
m_directServerAddress = serverAddr;
|
||||
m_directLocalAddress = localAddr;
|
||||
m_directCommandPort = commandPort;
|
||||
m_directDataPort = dataPort;
|
||||
m_directMulticastAddress = multicastAddr;
|
||||
}
|
||||
|
||||
private void UpdateDirectDefinitions()
|
||||
{
|
||||
IPAddress serverAddr;
|
||||
IPAddress localAddr;
|
||||
IPAddress multicastAddr = null;
|
||||
UInt16 commandPort;
|
||||
UInt16 dataPort;
|
||||
|
||||
try
|
||||
{
|
||||
serverAddr = IPAddress.Parse(ServerAddress);
|
||||
commandPort = (UInt16)Mathf.Clamp(CommandPort, 1, 65535);
|
||||
dataPort = (UInt16)Mathf.Clamp(DataPort, 1, 65535);
|
||||
if (!string.IsNullOrWhiteSpace(MulticastAddress))
|
||||
multicastAddr = IPAddress.Parse(MulticastAddress.Trim());
|
||||
localAddr = ResolveLocalAddress(serverAddr);
|
||||
ResolvedLocalAddress = localAddr.ToString();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
throw new InvalidOperationException("Error parsing direct NatNet refresh settings.", ex);
|
||||
}
|
||||
|
||||
string hostName;
|
||||
byte[] appVersion;
|
||||
byte[] natNetVersion;
|
||||
byte[] modelDefPacket;
|
||||
UInt16 negotiatedDataPort = dataPort;
|
||||
IPAddress negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
|
||||
|
||||
if (DirectRequestServerInfo(serverAddr, localAddr, commandPort, out hostName, out appVersion, out natNetVersion, out negotiatedDataPort, out negotiatedMulticast))
|
||||
{
|
||||
if (negotiatedDataPort == 0)
|
||||
negotiatedDataPort = dataPort;
|
||||
if (negotiatedMulticast == null)
|
||||
negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
|
||||
|
||||
ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3];
|
||||
}
|
||||
else
|
||||
{
|
||||
negotiatedDataPort = dataPort;
|
||||
negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
|
||||
}
|
||||
|
||||
if (!DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket))
|
||||
throw new InvalidOperationException("Direct NatNet MODELDEF request failed.");
|
||||
if (!DirectUpdateDefinitions(modelDefPacket))
|
||||
throw new InvalidOperationException("Direct NatNet MODELDEF parse failed.");
|
||||
|
||||
bool dataEndpointChanged =
|
||||
m_directServerAddress == null ||
|
||||
m_directLocalAddress == null ||
|
||||
m_directMulticastAddress == null ||
|
||||
!m_directServerAddress.Equals(serverAddr) ||
|
||||
!m_directLocalAddress.Equals(localAddr) ||
|
||||
!m_directMulticastAddress.Equals(negotiatedMulticast) ||
|
||||
m_directCommandPort != commandPort ||
|
||||
m_directDataPort != negotiatedDataPort;
|
||||
|
||||
if (dataEndpointChanged)
|
||||
{
|
||||
if (!StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort))
|
||||
throw new InvalidOperationException("Direct NatNet frame receiver failed to restart after endpoint change.");
|
||||
Debug.Log(GetType().FullName + ": direct NatNet frame endpoint changed during refresh; receiver restarted on " + negotiatedMulticast + ":" + negotiatedDataPort + ".", this);
|
||||
}
|
||||
|
||||
StoreDirectEndpoint(serverAddr, localAddr, commandPort, negotiatedDataPort, negotiatedMulticast);
|
||||
ClientNatNetVersion = "Direct UDP";
|
||||
}
|
||||
|
||||
private bool DirectRequestServerInfo(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, out string hostName, out byte[] appVersion, out byte[] natNetVersion, out UInt16 dataPort, out IPAddress multicastAddress)
|
||||
{
|
||||
hostName = "";
|
||||
@ -2431,7 +2661,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
byte[] response;
|
||||
byte[] pingPayload = DirectBuildPingPayload();
|
||||
if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 0, pingPayload, 2000, out response)) // NAT_PING
|
||||
if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 0, pingPayload, Mathf.Max(DirectServerInfoTimeoutMs, 100), out response)) // NAT_PING
|
||||
return false;
|
||||
if (response.Length < 4 || BitConverter.ToUInt16(response, 0) != 1) // NAT_PINGRESPONSE / server info
|
||||
return false;
|
||||
@ -2469,7 +2699,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
{
|
||||
modelDefPacket = null;
|
||||
byte[] response;
|
||||
if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 4, null, 3000, out response)) // NAT_REQUEST_MODELDEF
|
||||
if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 4, null, Mathf.Max(DirectModelDefTimeoutMs, 100), out response)) // NAT_REQUEST_MODELDEF
|
||||
return false;
|
||||
if (response.Length < 8 || BitConverter.ToUInt16(response, 0) != 5) // NAT_MODELDEF
|
||||
return false;
|
||||
@ -2583,6 +2813,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
}
|
||||
|
||||
PruneStatesMissingFromDefinitions();
|
||||
NotifyRegisteredSkeletonDefinitionsChanged();
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -2880,12 +3111,12 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
System.Collections.IEnumerator CheckConnectionHealth()
|
||||
{
|
||||
const float kHealthCheckIntervalSeconds = 1.0f;
|
||||
const float kRecentFrameThresholdSeconds = 5.0f;
|
||||
float healthCheckIntervalSeconds = Mathf.Max(ConnectionHealthCheckIntervalSeconds, 0.1f);
|
||||
float recentFrameThresholdSeconds = Mathf.Max(StreamingFrameTimeoutSeconds, healthCheckIntervalSeconds);
|
||||
|
||||
// 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 );
|
||||
YieldInstruction checkIntervalYield = new WaitForSeconds( healthCheckIntervalSeconds );
|
||||
OptitrackHiResTimer.Timestamp connectionInitiatedTimestamp = OptitrackHiResTimer.Now();
|
||||
bool wasReceivingFrames = false;
|
||||
bool warnedPendingFirstFrame = false;
|
||||
@ -2897,7 +3128,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
if ( m_receivedFrameSinceConnect == false && !IsReplayFrameFresh() )
|
||||
{
|
||||
// Still waiting for first frame. Warn exactly once if this takes too long.
|
||||
if ( connectionInitiatedTimestamp.AgeSeconds > kRecentFrameThresholdSeconds )
|
||||
if ( connectionInitiatedTimestamp.AgeSeconds > recentFrameThresholdSeconds )
|
||||
{
|
||||
if ( warnedPendingFirstFrame == false )
|
||||
{
|
||||
@ -2906,6 +3137,12 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
else
|
||||
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;
|
||||
|
||||
if ( AutoReconnect )
|
||||
{
|
||||
Debug.Log( GetType().FullName + ": starting automatic reconnect while waiting for the first streaming frame.", this );
|
||||
Reconnect();
|
||||
}
|
||||
}
|
||||
|
||||
continue;
|
||||
@ -2914,7 +3151,7 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
else
|
||||
{
|
||||
// We've received at least one frame, do ongoing checks for changes in connection health.
|
||||
bool receivedRecentFrame = HasRecentStreamingFrame(kRecentFrameThresholdSeconds);
|
||||
bool receivedRecentFrame = HasRecentStreamingFrame(recentFrameThresholdSeconds);
|
||||
|
||||
if ( wasReceivingFrames == false && receivedRecentFrame == true )
|
||||
{
|
||||
@ -2928,11 +3165,6 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
// Transition: Good health -> bad health.
|
||||
wasReceivingFrames = false;
|
||||
Debug.LogWarning( GetType().FullName + ": No streaming frames received from the server recently.", this );
|
||||
if (m_directNatNetConnected)
|
||||
{
|
||||
Debug.LogWarning(GetType().FullName + ": direct UDP receiver stays active; automatic reconnect is skipped to avoid interrupting live/replay multicast.", this);
|
||||
continue;
|
||||
}
|
||||
if ( AutoReconnect )
|
||||
{
|
||||
Debug.Log( GetType().FullName + ": starting automatic reconnect.", this );
|
||||
@ -3005,16 +3237,21 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBodyCount( pFrame, out frameRbCount );
|
||||
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBodyCount failed." );
|
||||
|
||||
m_currentFrameRigidBodyIds.Clear();
|
||||
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." );
|
||||
|
||||
m_currentFrameRigidBodyIds.Add(rbData.Id);
|
||||
|
||||
// Ensure we have a state corresponding to this rigid body ID.
|
||||
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState( rbData.Id );
|
||||
RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState);
|
||||
}
|
||||
if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot))
|
||||
QueueStreamedTopologyDefinitionRefresh();
|
||||
|
||||
// ----------------------
|
||||
// - Update skeletons
|
||||
@ -3023,12 +3260,15 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount( pFrame, out frameSkeletonCount );
|
||||
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetSkeletonCount failed." );
|
||||
|
||||
m_currentFrameSkeletonIds.Clear();
|
||||
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." );
|
||||
|
||||
m_currentFrameSkeletonIds.Add(skeletonId);
|
||||
|
||||
// Ensure we have a state corresponding to this skeleton ID.
|
||||
OptitrackSkeletonState skelState = GetOrCreateSkeletonState( skeletonId );
|
||||
|
||||
@ -3043,8 +3283,8 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
if (!m_pendingDefinitionRefresh)
|
||||
{
|
||||
Debug.LogWarning(GetType().FullName + ": missing skeleton definition for streamed skeleton ID " + skeletonId + "; scheduling definition refresh.", this);
|
||||
m_pendingDefinitionRefresh = true;
|
||||
}
|
||||
QueueStreamedTopologyDefinitionRefresh();
|
||||
continue;
|
||||
}
|
||||
|
||||
@ -3103,6 +3343,8 @@ public class OptitrackStreamingClient : MonoBehaviour
|
||||
|
||||
skelState.DeliveryTimestamp = frameTimestamp;
|
||||
}
|
||||
if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot))
|
||||
QueueStreamedTopologyDefinitionRefresh();
|
||||
|
||||
// -----------------------------------------------------
|
||||
// - Update trained markerset // trained markerset added
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user