Merge pull request #10 from stereolabs/fix_1_avatar_moving_per_frame

bufferize received udp data and debufferize each frame

Author: Bvallon-sl

README.md

@@ -115,6 +115,13 @@ These are located on the `ZED Skeleton Tracking Manager` script in the `Fusion M
 - `Enable SDK Skeleton` : controls the visibility of the stickman view of the SDK keypoints. Whereas the 3D avatar is animated using local rotations derived from the keypoints, this show the actual positions of the keypoints detected by the SDK.
 - `Log Fusion Metrics` : enables logging the metrics sent by the Fusion module in the console.
 
+### Troubleshooting
+
+If no skeleton data is received in Unity, you can either try to :
+
+- Disable your firewall.
+- Change the port used to send the data, it might already be used by another process.
+
 ## Support
 You will find guidance and assistance :
 - In the [documentation of the sample](https://www.stereolabs.com/docs/livelink/livelink-unity/)

ZEDUnityLivelink/Assets/ZEDFusion/Scripts/ZEDStreamingClient.cs

@@ -4,6 +4,8 @@
 using System.Net.Sockets;
 using System.Text;
 using UnityEngine;
+using System.Linq;
+using System;
 
 public class ZEDStreamingClient : MonoBehaviour
 {
@@ -17,28 +19,26 @@ public class ZEDStreamingClient : MonoBehaviour
     public string multicastIpAddress = "230.0.0.1";
 
     public bool showZEDFusionMetrics = false;
-    
+
     private object obj = null;
     private System.AsyncCallback AC;
     byte[] receivedBytes;
 
-    int bufferSize = 10;
-    LinkedList<byte[]> receivedDataBuffer;
-
     bool newDataAvailable = false;
     sl.DetectionData data;
 
     public delegate void onNewDetectionTriggerDelegate(sl.Bodies bodies);
     public event onNewDetectionTriggerDelegate OnNewDetection;
 
+    object mutex_buffer = new object();
+    SortedDictionary<ulong, List<sl.DetectionData>> detectionDataDict = new SortedDictionary<ulong, List<sl.DetectionData>>();
+
     void Start()
     {
         InitializeUDPListener();
     }
     public void InitializeUDPListener()
     {
-        receivedDataBuffer = new LinkedList<byte[]>();
-
         ipEndPointData = new IPEndPoint(IPAddress.Any, port);
         clientData = new UdpClient();
         clientData.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, optionValue: true);
@@ -60,34 +60,101 @@ public void InitializeUDPListener()
 
     void ReceivedUDPPacket(System.IAsyncResult result)
     {
-        //stopwatch.Start();
         receivedBytes = clientData.EndReceive(result, ref ipEndPointData);
         ParsePacket();
         clientData.BeginReceive(AC, obj);
     } // ReceiveCallBack
 
+    // fill detectionDataDict with received data
     void ParsePacket()
     {
-        if (receivedDataBuffer.Count == bufferSize) receivedDataBuffer.RemoveFirst();
-        receivedDataBuffer.AddLast(receivedBytes);
-        newDataAvailable = true;
+        sl.DetectionData d = sl.DetectionData.CreateFromJSON(receivedBytes);
+
+        // initialize data if necessary
+        if (data==null) { data = sl.DetectionData.CreateFromJSON(receivedBytes); }
+
+        lock (mutex_buffer) // lock dictionary.
+        {
+            // add to data dictionary
+            if (detectionDataDict.ContainsKey(d.bodies.timestamp)) // if key (timestamp) exists
+            {
+                detectionDataDict.GetValueOrDefault(d.bodies.timestamp).Add(d);
+            }
+            else // key (timestamp) does not exist yet
+            {
+                List<sl.DetectionData> tmpL = new List<sl.DetectionData> { d };
+                detectionDataDict.Add(d.bodies.timestamp, tmpL);
+            }
+        }
     }
 
-    public bool IsNewDataAvailable()
+    // merge bodies data from dictionary into 1 bodies data
+    public void PrepareData()
     {
-        return newDataAvailable;
+        if(data!=null)
+        {
+            lock (mutex_buffer) // lock dictionary.
+            {
+                if (detectionDataDict.Count > 0)
+                {
+                    sl.Bodies bodies = new sl.Bodies();
+
+                    // get oldest timestamp from dict
+                    bodies.timestamp = detectionDataDict.First().Key;
+
+                    // get a ref bodies
+                    sl.Bodies refBodies = detectionDataDict.First().Value.First().bodies;
+
+                    //initialize body_list
+                    bodies.body_list = new sl.BodyData[detectionDataDict.First().Value.Count];
+
+                    // fill bodies.body_list with list of detection data for said timestamp
+                    for (int i = 0; i < bodies.body_list.Length; ++i)
+                    {
+                        bodies.body_list[i] = detectionDataDict.First().Value[i].bodies.body_list[0];
+                    }
+
+                    // fill additional data
+                    data.fusionMetrics = detectionDataDict.First().Value.First().fusionMetrics;
+                    bodies.body_format = refBodies.body_format;
+                    bodies.is_tracked = refBodies.is_tracked;
+                    bodies.nb_object = refBodies.nb_object;
+                    bodies.is_new = refBodies.is_new;
+
+                    data.bodies = bodies;
+
+                    // remove from dictionary
+                    detectionDataDict.Remove(bodies.timestamp);
+
+
+                    newDataAvailable = true;
+                }
+                else
+                {
+                    data.bodies.is_new = 0;
+                }
+            }
+        }        
     }
 
-    public sl.Bodies GetLastBodiesData()
+    public bool IsNewDataAvailable()
     {
-       data = sl.DetectionData.CreateFromJSON(receivedDataBuffer.Last.Value);
-
-       return data.bodies;
+        return newDataAvailable;
     }
 
     public sl.FusionMetrics GetLastFusionMetrics()
     {
-        return data.fusionMetrics;
+        lock (mutex_buffer)
+        {
+            return data.fusionMetrics;
+        }
+    }
+    public sl.Bodies GetLastBodies()
+    {
+        lock (mutex_buffer)
+        {
+            return data.bodies;
+        }
     }
 
     public bool ShowFusionMetrics()
@@ -98,9 +165,11 @@ public bool ShowFusionMetrics()
 
     private void Update()
     {
+        PrepareData();
         if (IsNewDataAvailable())
         {
-            OnNewDetection(GetLastBodiesData());
+            OnNewDetection(GetLastBodies());
+
             newDataAvailable = false;
 
             if (ShowFusionMetrics())

zed-unity-livelink-mono/src/PracticalSocket.cpp

@@ -18,6 +18,7 @@
  */
 
 #include "PracticalSocket.h"
+#include <cstring>
 
 #ifdef WIN32
 #include <winsock.h>         // For socket(), connect(), send(), and recv()