Improve OptiTrack reconnect definition refresh

This commit is contained in:
DESKTOP-S4BOTN2\user 2026-06-21 22:29:24 +09:00
parent c55103a3e5
commit 407c20470e
5 changed files with 335 additions and 42 deletions

View File

@ -16,6 +16,7 @@ public class OptitrackStreamingClientEditor : Editor
private VisualElement runtimeOffline; private VisualElement runtimeOffline;
private VisualElement runtimeOnline; private VisualElement runtimeOnline;
private VisualElement runtimeInfo; private VisualElement runtimeInfo;
private Label runtimeActionText;
public override VisualElement CreateInspectorGUI() public override VisualElement CreateInspectorGUI()
{ {
@ -36,10 +37,11 @@ public class OptitrackStreamingClientEditor : Editor
runtimeOffline = root.Q("runtimeOffline"); runtimeOffline = root.Q("runtimeOffline");
runtimeOnline = root.Q("runtimeOnline"); runtimeOnline = root.Q("runtimeOnline");
runtimeInfo = root.Q("runtimeInfo"); runtimeInfo = root.Q("runtimeInfo");
runtimeActionText = root.Q<Label>("runtimeActionText");
var reconnectBtn = root.Q<Button>("reconnectBtn"); var reconnectBtn = root.Q<Button>("reconnectBtn");
if (reconnectBtn != null) if (reconnectBtn != null)
reconnectBtn.clicked += () => { if (Application.isPlaying) client.Reconnect(); }; reconnectBtn.clicked += () => RunPlayModeAction("Full reconnect requested", () => client.Reconnect());
root.schedule.Execute(UpdatePlayModeState).Every(300); root.schedule.Execute(UpdatePlayModeState).Every(300);
@ -83,9 +85,21 @@ public class OptitrackStreamingClientEditor : Editor
SetStatusStyle(null); SetStatusStyle(null);
if (statusText != null) if (statusText != null)
statusText.text = "Stopped"; 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) private void SetStatusStyle(bool? connected)
{ {
if (statusDot == null || statusText == null) return; if (statusDot == null || statusText == null) return;

View File

@ -95,6 +95,13 @@
background-color: #b45309; background-color: #b45309;
} }
.opti-runtime-action {
font-size: 11px;
color: #fbbf24;
-unity-font-style: bold;
margin-bottom: 4px;
}
.opti-runtime-info { .opti-runtime-info {
padding: 4px 0; padding: 4px 0;
} }

View File

@ -41,6 +41,20 @@
</ui:Foldout> </ui:Foldout>
</ui:VisualElement> </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 --> <!-- Definition Refresh -->
<ui:VisualElement class="section"> <ui:VisualElement class="section">
<ui:Foldout text="Definition Refresh" value="true" class="section-foldout"> <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:HelpBox message-type="Info" text="Runtime controls are available in Play Mode."/>
</ui:VisualElement> </ui:VisualElement>
<ui:VisualElement name="runtimeOnline" class="opti-runtime-online"> <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 name="runtimeInfo" class="opti-runtime-info"/>
</ui:VisualElement> </ui:VisualElement>
</ui:Foldout> </ui:Foldout>

View File

@ -47,8 +47,8 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
public float interpolationDelay = 0f; public float interpolationDelay = 0f;
private OptitrackSkeletonDefinition m_skeletonDef; private OptitrackSkeletonDefinition m_skeletonDef;
private int m_boundSkeletonDefinitionId = -1;
private string previousSkeletonName; private string previousSkeletonName;
private OptitrackStreamingClient m_boundStreamingClient; private OptitrackStreamingClient m_boundStreamingClient;
[HideInInspector] [HideInInspector]
@ -295,10 +295,19 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
RebindStreamingClient(); RebindStreamingClient();
} }
public void RefreshSkeletonDefinitionBinding()
{
if (StreamingClient == null)
InitializeStreamingClient();
RebindStreamingClient();
}
private void RebindStreamingClient() private void RebindStreamingClient()
{ {
m_boundStreamingClient = StreamingClient; m_boundStreamingClient = StreamingClient;
m_skeletonDef = null; m_skeletonDef = null;
m_boundSkeletonDefinitionId = -1;
m_boneIdToMappingIndex.Clear(); m_boneIdToMappingIndex.Clear();
m_snapshotPositions.Clear(); m_snapshotPositions.Clear();
m_snapshotOrientations.Clear(); m_snapshotOrientations.Clear();
@ -314,6 +323,7 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName); m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
if (m_skeletonDef != null) if (m_skeletonDef != null)
{ {
m_boundSkeletonDefinitionId = m_skeletonDef.Id;
RebuildBoneIdMapping(); RebuildBoneIdMapping();
isSkeletonFound = true; isSkeletonFound = true;
} }
@ -595,18 +605,21 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
{ {
if (StreamingClient != null) 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); OptitrackSkeletonState skelState = StreamingClient.GetLatestSkeletonState(m_skeletonDef.Id);
bool wasFound = isSkeletonFound; bool wasFound = isSkeletonFound;
isSkeletonFound = (skelState != null); isSkeletonFound = (skelState != null);
if (isSkeletonFound && !wasFound) if (isSkeletonFound && (!wasFound || definitionChanged))
{ {
StreamingClient.RegisterSkeleton(this, SkeletonAssetName); StreamingClient.RegisterSkeleton(this, SkeletonAssetName);
previousSkeletonName = SkeletonAssetName; previousSkeletonName = SkeletonAssetName;
m_boundSkeletonDefinitionId = m_skeletonDef.Id;
RebuildBoneIdMapping(); RebuildBoneIdMapping();
Debug.Log($"[OptiTrack] 스켈레톤 '{SkeletonAssetName}' 연결 성공"); Debug.Log($"[OptiTrack] 스켈레톤 '{SkeletonAssetName}' 연결 성공");
} }
@ -622,6 +635,8 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
{ {
// 스켈레톤 정의를 찾지 못함 → 지수 백오프로 재조회 요청 빈도 제한 // 스켈레톤 정의를 찾지 못함 → 지수 백오프로 재조회 요청 빈도 제한
isSkeletonFound = false; isSkeletonFound = false;
m_skeletonDef = null;
m_boundSkeletonDefinitionId = -1;
m_definitionRefreshRequests++; m_definitionRefreshRequests++;
// 처음 3회까지는 즉시 요청, 이후 백오프 적용 // 처음 3회까지는 즉시 요청, 이후 백오프 적용

View File

@ -123,6 +123,9 @@ public class OptitrackSkeletonDefinition
/// <summary>Skeleton asset name.</summary> /// <summary>Skeleton asset name.</summary>
public string Name; 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> /// <summary>Bone names, hierarchy, and neutral pose position information.</summary>
public List<BoneDefinition> Bones; public List<BoneDefinition> Bones;
@ -348,6 +351,35 @@ public class OptitrackStreamingClient : MonoBehaviour
[Tooltip("Automatically retries the initial connection and reconnects when streaming frames stop.")] [Tooltip("Automatically retries the initial connection and reconnects when streaming frames stop.")]
public bool AutoReconnect = true; 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")] [Tooltip("Changes to the version of Natnet used by the server")]
public string ServerNatNetVersion = ""; public string ServerNatNetVersion = "";
public string ClientNatNetVersion = ""; public string ClientNatNetVersion = "";
@ -385,6 +417,11 @@ public class OptitrackStreamingClient : MonoBehaviour
private int m_directTimeoutCount = 0; private int m_directTimeoutCount = 0;
private bool m_directNatNetConnected = false; private bool m_directNatNetConnected = false;
private bool m_directNoModelDefWarned = 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_client;
private NatNetClient m_replayClient; private NatNetClient m_replayClient;
@ -426,6 +463,7 @@ public class OptitrackStreamingClient : MonoBehaviour
/// <summary>Maps from a streamed skeleton names to its component.</summary> /// <summary>Maps from a streamed skeleton names to its component.</summary>
private Dictionary<string, MonoBehaviour> m_skeletons = new Dictionary<string, MonoBehaviour>(); 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> /// <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 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> /// <summary>Cache from streamed asset ID to asset name.</summary>
private Dictionary<Int32, string> m_assetIdToNameCache = new Dictionary<Int32, string>(); 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_pendingDefinitionRefresh = false;
private volatile bool m_forceDefinitionRefreshSoon = false;
private volatile bool m_pendingSkeletonDefinitionNotify = false;
private float m_definitionRefreshCooldown = 0f; private float m_definitionRefreshCooldown = 0f;
private bool m_isReconnecting = false; private bool m_isReconnecting = false;
@ -473,8 +520,39 @@ public class OptitrackStreamingClient : MonoBehaviour
m_pendingDefinitionRefresh = true; 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() private void Update()
{ {
if (m_pendingSkeletonDefinitionNotify)
{
m_pendingSkeletonDefinitionNotify = false;
NotifyRegisteredSkeletonDefinitionsChanged();
}
if (m_directFrameLogPending) if (m_directFrameLogPending)
{ {
m_directFrameLogPending = false; m_directFrameLogPending = false;
@ -683,14 +761,20 @@ public class OptitrackStreamingClient : MonoBehaviour
m_latestTMarkMarkerSpheres.Clear(); 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. // 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; m_pendingDefinitionRefresh = false;
try try
{ {
UpdateDefinitions(); RefreshDefinitionsForActiveTransport();
ResubscribeRegisteredAssets();
m_definitionRefreshCooldown = Mathf.Max(DefinitionRefreshInterval, 1f); m_definitionRefreshCooldown = Mathf.Max(DefinitionRefreshInterval, 1f);
m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f); m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
} }
@ -706,8 +790,7 @@ public class OptitrackStreamingClient : MonoBehaviour
m_definitionRefreshCooldown -= Time.deltaTime; m_definitionRefreshCooldown -= Time.deltaTime;
if (AutoRefreshDefinitions && if (AutoRefreshDefinitions &&
m_client != null && CanRefreshDefinitions() &&
!SkipDataDescriptions &&
m_definitionRefreshCooldown <= 0f && m_definitionRefreshCooldown <= 0f &&
Time.unscaledTime >= m_nextAutoDefinitionRefreshTime) Time.unscaledTime >= m_nextAutoDefinitionRefreshTime)
{ {
@ -780,13 +863,7 @@ public class OptitrackStreamingClient : MonoBehaviour
return; return;
} }
if (m_directNatNetConnected && ConnectionType == ClientConnectionType.Multicast) Debug.Log("OptiTrack: full reconnect requested.");
{
Debug.Log(GetType().FullName + ": direct UDP receiver is already active; reconnect request skipped.", this);
return;
}
Debug.Log("OptiTrack: reconnect requested.");
OnDisable(); OnDisable();
@ -795,7 +872,7 @@ public class OptitrackStreamingClient : MonoBehaviour
/// <summary> /// <summary>
/// Coroutine for handling the reconnection process with a retry loop. /// 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). /// Last known pose is preserved during reconnect (m_latestSkeletonStates not cleared).
/// </summary> /// </summary>
private System.Collections.IEnumerator ReconnectCoroutine() private System.Collections.IEnumerator ReconnectCoroutine()
@ -806,13 +883,16 @@ public class OptitrackStreamingClient : MonoBehaviour
for (int attempt = 1; attempt <= maxAttempts; attempt++) for (int attempt = 1; attempt <= maxAttempts; attempt++)
{ {
m_receivedFrameSinceConnect = false; 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); Debug.Log(string.Format("{0}: reconnect attempt {1}/{2} in {3} seconds...", GetType().FullName, attempt, maxAttempts, delay), this);
yield return new WaitForSeconds(delay); if (delay > 0f)
yield return new WaitForSeconds(delay);
yield return StartCoroutine(ConnectCoroutine()); 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) while (!m_receivedFrameSinceConnect && Time.realtimeSinceStartup < deadline)
yield return null; yield return null;
@ -1231,6 +1311,9 @@ public class OptitrackStreamingClient : MonoBehaviour
{ {
OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i]; OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i];
if (skelDef.IsSynthetic)
continue;
if ( skelDef.Name.Equals( skeletonAssetName, StringComparison.InvariantCultureIgnoreCase ) ) if ( skelDef.Name.Equals( skeletonAssetName, StringComparison.InvariantCultureIgnoreCase ) )
{ {
return skelDef; return skelDef;
@ -1577,6 +1660,7 @@ public class OptitrackStreamingClient : MonoBehaviour
} }
PruneStatesMissingFromDefinitions(); PruneStatesMissingFromDefinitions();
NotifyRegisteredSkeletonDefinitionsChanged();
} }
@ -1639,6 +1723,25 @@ public class OptitrackStreamingClient : MonoBehaviour
SubscribeMarkers(); 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 ) public void RegisterRigidBody( MonoBehaviour component, Int32 rigidBodyId )
{ {
if ( m_rigidBodies.TryGetValue( rigidBodyId, out MonoBehaviour existingComponent ) ) if ( m_rigidBodies.TryGetValue( rigidBodyId, out MonoBehaviour existingComponent ) )
@ -1673,6 +1776,8 @@ public class OptitrackStreamingClient : MonoBehaviour
public void RegisterSkeleton(MonoBehaviour component, string name) public void RegisterSkeleton(MonoBehaviour component, string name)
{ {
m_registeredSkeletonComponents.Add(component);
if (m_skeletons.TryGetValue(name, out MonoBehaviour existingComponent)) if (m_skeletons.TryGetValue(name, out MonoBehaviour existingComponent))
{ {
if (existingComponent == component) if (existingComponent == component)
@ -1899,7 +2004,7 @@ public class OptitrackStreamingClient : MonoBehaviour
return liveFresh || replayFresh; return liveFresh || replayFresh;
} }
private void StartDirectFrameReceiver(string localAddress, string multicastAddress, UInt16 dataPort) private bool StartDirectFrameReceiver(string localAddress, string multicastAddress, UInt16 dataPort)
{ {
StopDirectFrameReceiver(); StopDirectFrameReceiver();
@ -1942,11 +2047,13 @@ public class OptitrackStreamingClient : MonoBehaviour
}; };
m_directFrameThread.Start(); 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); 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) catch (Exception ex)
{ {
StopDirectFrameReceiver(); StopDirectFrameReceiver();
Debug.LogWarning(GetType().FullName + ": direct NatNet frame receiver failed to start: " + ex.Message, this); 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; 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) private void ParseDirectRigidBodies(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp)
{ {
int o = start; int o = start;
for (int i = 0; i < count && o < end; i++) m_currentFrameRigidBodyIds.Clear();
for (int i = 0; i < count; i++)
{ {
sRigidBodyData rbData; sRigidBodyData rbData;
if (!ReadDirectRigidBody(data, ref o, end, out rbData)) return; if (!ReadDirectRigidBody(data, ref o, end, out rbData)) return;
m_currentFrameRigidBodyIds.Add(rbData.Id);
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id); OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id);
RigidBodyDataToState(rbData, frameTimestamp, rbState); 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) private void ParseDirectSkeletons(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp)
{ {
int o = start; 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 skeletonId = BitConverter.ToInt32(data, o); o += 4;
int boneCount = BitConverter.ToInt32(data, o); o += 4; int boneCount = BitConverter.ToInt32(data, o); o += 4;
if (boneCount < 0 || boneCount > 4096 || o + boneCount * 38 > end) return; if (boneCount < 0 || boneCount > 4096 || o + boneCount * 38 > end) return;
m_currentFrameSkeletonIds.Add(skeletonId);
sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, boneCount); sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, boneCount);
for (int b = 0; b < boneCount; b++) for (int b = 0; b < boneCount; b++)
@ -2124,6 +2268,9 @@ public class OptitrackStreamingClient : MonoBehaviour
CommitDirectSkeletonFrame(skeletonId, stagedBones, boneCount, frameTimestamp); 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) private void CommitDirectSkeletonFrame(int skeletonId, sRigidBodyData[] stagedBones, int boneCount, OptitrackHiResTimer.Timestamp frameTimestamp)
@ -2136,8 +2283,7 @@ public class OptitrackStreamingClient : MonoBehaviour
if (skelDef == null) if (skelDef == null)
{ {
if (!m_pendingDefinitionRefresh) QueueStreamedTopologyDefinitionRefresh();
m_pendingDefinitionRefresh = true;
return; return;
} }
@ -2208,6 +2354,7 @@ public class OptitrackStreamingClient : MonoBehaviour
{ {
Id = skeletonId, Id = skeletonId,
Name = skeletonName, Name = skeletonName,
IsSynthetic = true,
Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(boneCount), Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(boneCount),
BoneIdToParentIdMap = new Dictionary<int, int>(), BoneIdToParentIdMap = new Dictionary<int, int>(),
}; };
@ -2237,6 +2384,7 @@ public class OptitrackStreamingClient : MonoBehaviour
m_skeletonDefinitions.Add(skelDef); m_skeletonDefinitions.Add(skelDef);
m_mirrorBoneIdMaps.Clear(); m_mirrorBoneIdMaps.Clear();
m_pendingSkeletonDefinitionNotify = true;
return skelDef; return skelDef;
} }
@ -2415,12 +2563,94 @@ public class OptitrackStreamingClient : MonoBehaviour
ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3]; ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3];
ClientNatNetVersion = "Direct UDP"; ClientNatNetVersion = "Direct UDP";
m_directNatNetConnected = true; if (!StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort))
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); Debug.Log(GetType().FullName + ": Connected to direct NatNet server. Server=" + serverAddr + ", host=" + hostName + ", local=" + localAddr + ", serverNatNet=" + ServerNatNetVersion + ", multicast=" + negotiatedMulticast + ":" + negotiatedDataPort + ".", this);
return true; 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) 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 = ""; hostName = "";
@ -2431,7 +2661,7 @@ public class OptitrackStreamingClient : MonoBehaviour
byte[] response; byte[] response;
byte[] pingPayload = DirectBuildPingPayload(); 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; return false;
if (response.Length < 4 || BitConverter.ToUInt16(response, 0) != 1) // NAT_PINGRESPONSE / server info if (response.Length < 4 || BitConverter.ToUInt16(response, 0) != 1) // NAT_PINGRESPONSE / server info
return false; return false;
@ -2469,7 +2699,7 @@ public class OptitrackStreamingClient : MonoBehaviour
{ {
modelDefPacket = null; modelDefPacket = null;
byte[] response; 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; return false;
if (response.Length < 8 || BitConverter.ToUInt16(response, 0) != 5) // NAT_MODELDEF if (response.Length < 8 || BitConverter.ToUInt16(response, 0) != 5) // NAT_MODELDEF
return false; return false;
@ -2583,6 +2813,7 @@ public class OptitrackStreamingClient : MonoBehaviour
} }
PruneStatesMissingFromDefinitions(); PruneStatesMissingFromDefinitions();
NotifyRegisteredSkeletonDefinitionsChanged();
return true; return true;
} }
@ -2880,12 +3111,12 @@ public class OptitrackStreamingClient : MonoBehaviour
System.Collections.IEnumerator CheckConnectionHealth() System.Collections.IEnumerator CheckConnectionHealth()
{ {
const float kHealthCheckIntervalSeconds = 1.0f; float healthCheckIntervalSeconds = Mathf.Max(ConnectionHealthCheckIntervalSeconds, 0.1f);
const float kRecentFrameThresholdSeconds = 5.0f; float recentFrameThresholdSeconds = Mathf.Max(StreamingFrameTimeoutSeconds, healthCheckIntervalSeconds);
// The lifespan of these variables is tied to the lifespan of a single connection session. // 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. // 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(); OptitrackHiResTimer.Timestamp connectionInitiatedTimestamp = OptitrackHiResTimer.Now();
bool wasReceivingFrames = false; bool wasReceivingFrames = false;
bool warnedPendingFirstFrame = false; bool warnedPendingFirstFrame = false;
@ -2897,7 +3128,7 @@ public class OptitrackStreamingClient : MonoBehaviour
if ( m_receivedFrameSinceConnect == false && !IsReplayFrameFresh() ) if ( m_receivedFrameSinceConnect == false && !IsReplayFrameFresh() )
{ {
// Still waiting for first frame. Warn exactly once if this takes too long. // Still waiting for first frame. Warn exactly once if this takes too long.
if ( connectionInitiatedTimestamp.AgeSeconds > kRecentFrameThresholdSeconds ) if ( connectionInitiatedTimestamp.AgeSeconds > recentFrameThresholdSeconds )
{ {
if ( warnedPendingFirstFrame == false ) if ( warnedPendingFirstFrame == false )
{ {
@ -2906,6 +3137,12 @@ public class OptitrackStreamingClient : MonoBehaviour
else 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 ); 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; warnedPendingFirstFrame = true;
if ( AutoReconnect )
{
Debug.Log( GetType().FullName + ": starting automatic reconnect while waiting for the first streaming frame.", this );
Reconnect();
}
} }
continue; continue;
@ -2914,7 +3151,7 @@ public class OptitrackStreamingClient : MonoBehaviour
else else
{ {
// We've received at least one frame, do ongoing checks for changes in connection health. // 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 ) if ( wasReceivingFrames == false && receivedRecentFrame == true )
{ {
@ -2928,11 +3165,6 @@ public class OptitrackStreamingClient : MonoBehaviour
// Transition: Good health -> bad health. // Transition: Good health -> bad health.
wasReceivingFrames = false; wasReceivingFrames = false;
Debug.LogWarning( GetType().FullName + ": No streaming frames received from the server recently.", this ); 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 ) if ( AutoReconnect )
{ {
Debug.Log( GetType().FullName + ": starting automatic reconnect.", this ); 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 ); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBodyCount( pFrame, out frameRbCount );
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBodyCount failed." ); NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBodyCount failed." );
m_currentFrameRigidBodyIds.Clear();
for (int rbIdx = 0; rbIdx < frameRbCount; ++rbIdx) for (int rbIdx = 0; rbIdx < frameRbCount; ++rbIdx)
{ {
sRigidBodyData rbData = new sRigidBodyData(); sRigidBodyData rbData = new sRigidBodyData();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody( pFrame, rbIdx, out rbData ); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody( pFrame, rbIdx, out rbData );
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBody failed." ); NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBody failed." );
m_currentFrameRigidBodyIds.Add(rbData.Id);
// Ensure we have a state corresponding to this rigid body ID. // Ensure we have a state corresponding to this rigid body ID.
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState( rbData.Id ); OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState( rbData.Id );
RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState); RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState);
} }
if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot))
QueueStreamedTopologyDefinitionRefresh();
// ---------------------- // ----------------------
// - Update skeletons // - Update skeletons
@ -3023,12 +3260,15 @@ public class OptitrackStreamingClient : MonoBehaviour
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount( pFrame, out frameSkeletonCount ); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount( pFrame, out frameSkeletonCount );
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetSkeletonCount failed." ); NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetSkeletonCount failed." );
m_currentFrameSkeletonIds.Clear();
for (int skelIdx = 0; skelIdx < frameSkeletonCount; ++skelIdx) for (int skelIdx = 0; skelIdx < frameSkeletonCount; ++skelIdx)
{ {
Int32 skeletonId; Int32 skeletonId;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetId( pFrame, skelIdx, out skeletonId ); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetId( pFrame, skelIdx, out skeletonId );
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetId failed." ); NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetId failed." );
m_currentFrameSkeletonIds.Add(skeletonId);
// Ensure we have a state corresponding to this skeleton ID. // Ensure we have a state corresponding to this skeleton ID.
OptitrackSkeletonState skelState = GetOrCreateSkeletonState( skeletonId ); OptitrackSkeletonState skelState = GetOrCreateSkeletonState( skeletonId );
@ -3043,8 +3283,8 @@ public class OptitrackStreamingClient : MonoBehaviour
if (!m_pendingDefinitionRefresh) if (!m_pendingDefinitionRefresh)
{ {
Debug.LogWarning(GetType().FullName + ": missing skeleton definition for streamed skeleton ID " + skeletonId + "; scheduling definition refresh.", this); Debug.LogWarning(GetType().FullName + ": missing skeleton definition for streamed skeleton ID " + skeletonId + "; scheduling definition refresh.", this);
m_pendingDefinitionRefresh = true;
} }
QueueStreamedTopologyDefinitionRefresh();
continue; continue;
} }
@ -3103,6 +3343,8 @@ public class OptitrackStreamingClient : MonoBehaviour
skelState.DeliveryTimestamp = frameTimestamp; skelState.DeliveryTimestamp = frameTimestamp;
} }
if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot))
QueueStreamedTopologyDefinitionRefresh();
// ----------------------------------------------------- // -----------------------------------------------------
// - Update trained markerset // trained markerset added // - Update trained markerset // trained markerset added