public void SendMessageToAllConnections(NetMessageData message)
{
foreach (NetConnData conn in connections)
{
lock (conn.messageQueue)
{
if (conn.messageQueue.Count < NetConnData.MaxMessageQueueLength)
{
conn.messageQueue.Enqueue(message);
}
}
}
}
// sends control message to the server
private void SendControlMessage(ControlMessageType messageType)
{
if (controlFrameServer != null && controlFrameServer.IsActive())
{
ulong ulTimeNow = (ulong)System.DateTime.Now.Ticks;
NetMessageData msg = new NetMessageData(NetMessageType.Control, FrameEncodeType.Raw, 0, 0, ulTimeNow);
byte[] btMsgData = new byte[1];
btMsgData[0] = (byte)messageType;
msg.SetData(btMsgData);
controlFrameServer.SendMessageToAllConnections(msg);
//Debug.Log("Sending ctrl message " + messageType);
}
}
public void SendMessageToConnection(NetMessageData message, int connId)
{
foreach (NetConnData conn in connections)
{
if (conn.ID == connId)
{
lock (conn.messageQueue)
{
if (conn.messageQueue.Count < NetConnData.MaxMessageQueueLength)
{
conn.messageQueue.Enqueue(message);
}
}
break;
}
}
}
// sends the net-sensor-data to clients
private void CtrlSendGetSensorData()
{
KinectInterop.NetSensorData netSensorData = sensorData.sensorInterface.GetNetSensorData(sensorData);
string sNetSensorData = netSensorData != null?JsonUtility.ToJson(netSensorData) : null;
byte[] btNetSensorData = System.Text.Encoding.UTF8.GetBytes(sNetSensorData);
ulong ulTimeNow = (ulong)System.DateTime.Now.Ticks;
NetMessageData message = new NetMessageData(NetMessageType.Control, FrameEncodeType.SensorDataJson, 0, 0, ulTimeNow);
message.SetData(btNetSensorData);
if (compressRawFrames)
{
message.SetCompressor(controlFrameCompressor);
}
controlFrameServer.SendMessageToAllConnections(message);
}
// sends the net-cst data to clients
private void CtrlSendGetCST()
{
Vector3[] avSpaceTable = sensorData.sensorInterface.GetColorCameraSpaceTable(sensorData);
if (avSpaceTable != null)
{
byte[] btSpaceTable = new byte[avSpaceTable.Length * 3 * sizeof(float)];
KinectInterop.CopyBytes(avSpaceTable, 3 * sizeof(float), btSpaceTable, sizeof(byte));
ulong ulTimeNow = (ulong)System.DateTime.Now.Ticks;
NetMessageData message = new NetMessageData(NetMessageType.Control, FrameEncodeType.CSTraw, sensorData.colorImageWidth, sensorData.colorImageHeight, ulTimeNow);
message.SetData(btSpaceTable);
if (compressRawFrames)
{
message.SetCompressor(controlFrameCompressor);
}
controlFrameServer.SendMessageToAllConnections(message);
}
}
private void ProcessReceivedData(byte[] buffer, int bytesReceived, NetConnData conn)
{
if (conn.bytesReceived == 0 && bytesReceived > 3) //new message
{
int newMessageSize = System.BitConverter.ToInt32(buffer, 0) + sizeof(int); //prefix is one int
if (newMessageSize != conn.messageSize)
{
conn.messageSize = newMessageSize;
conn.messageBuffer = new byte[newMessageSize];
}
}
int availableLength = conn.messageSize - conn.bytesReceived;
int copyLen = Mathf.Min(availableLength, bytesReceived);
conn.packetCounter++;
System.Array.Copy(buffer, 0, conn.messageBuffer, conn.bytesReceived, copyLen);
conn.bytesReceived += copyLen;
if (conn.bytesReceived == conn.messageSize)
{
conn.ResetCounters();
NetMessageData message = new NetMessageData();
//message.SetDecompressor(decompressor);
message.UnwrapMessage(conn.messageBuffer);
ReceivedMessage?.Invoke(conn, new ReceivedMessageEventArgs(message));
}
if (copyLen != bytesReceived)
{
byte[] newBuffer = new byte[bytesReceived - copyLen];
System.Array.Copy(buffer, copyLen, newBuffer, 0, bytesReceived - copyLen);
ProcessReceivedData(newBuffer, bytesReceived - copyLen, conn);
}
}
public ReceivedMessageEventArgs(NetMessageData message, bool synched)
{
this.message = message;
this.synched = synched;
}
public ReceivedMessageEventArgs(NetMessageData message)
{
this.message = message;
this.synched = false;
}
// checks for updated sensor data and sends it to the connected clients
private void UpdateNetServers()
{
if (sensorData == null)
{
return;
}
try
{
// control - keep alive
ulong ulTimeNow = (ulong)System.DateTime.Now.Ticks;
bool isControlClientActive = controlFrameServer != null?controlFrameServer.IsActive() : false;
if ((ulTimeNow - lastKeepAliveFrameTime) >= keepAliveInterval && isControlClientActive)
{
lastKeepAliveFrameTime = ulTimeNow;
SendControlMessage(ControlMessageType.KeepAlive);
}
// color frame
if (colorFrameServer != null && colorFrameServer.GetConnCount() > 0)
{
if (sensorData.lastColorFrameTime != lastColorFrameTime)
{
lastColorFrameTime = sensorData.lastColorFrameTime;
byte[] btColorData = ((Texture2D)sensorData.colorImageTexture).EncodeToJPG();
NetMessageData message = new NetMessageData(NetMessageType.Color, FrameEncodeType.Jpeg,
sensorData.colorImageWidth, sensorData.colorImageHeight, lastColorFrameTime);
message.SetData(btColorData);
colorFrameServer.SendMessageToAllConnections(message);
}
}
// depth frame
if (depthFrameServer != null && depthFrameServer.GetConnCount() > 0)
{
if (sensorData.lastDepthFrameTime != lastDepthFrameTime)
{
lastDepthFrameTime = sensorData.lastDepthFrameTime;
NetMessageData message = new NetMessageData(NetMessageType.Depth, FrameEncodeType.Raw,
sensorData.depthImageWidth, sensorData.depthImageHeight, lastDepthFrameTime);
message.SetData(sensorData.depthImage);
if (compressRawFrames)
{
message.SetCompressor(depthFrameCompressor);
}
depthFrameServer.SendMessageToAllConnections(message);
}
}
// infrared frame
if (infraredFrameServer != null && infraredFrameServer.GetConnCount() > 0)
{
if (sensorData.lastInfraredImageTime != lastInfraredFrameTime)
{
lastInfraredFrameTime = sensorData.lastInfraredImageTime;
byte[] btInfraredData = GetTextureAsJpeg(sensorData.infraredImageTexture, infraredTex2d);
NetMessageData message = new NetMessageData(NetMessageType.Color, FrameEncodeType.Jpeg,
sensorData.depthImageWidth, sensorData.depthImageHeight, lastInfraredFrameTime);
message.SetData(btInfraredData);
infraredFrameServer.SendMessageToAllConnections(message);
}
}
// body data frame
if (bodyDataFrameServer != null && bodyDataFrameServer.GetConnCount() > 0)
{
if (sensorData.lastBodyFrameTime != lastBodyDataFrameTime)
{
lastBodyDataFrameTime = sensorData.lastBodyFrameTime;
float fUnityTime = 0f;
string sBodyFrame = KinectInterop.GetBodyFrameAsCsv(ref sensorData.alTrackedBodies,
sensorData.trackedBodiesCount, sensorData.lastBodyFrameTime, sensorData.sensorSpaceScale, ref fUnityTime, '\t');
byte[] btBodyData = System.Text.Encoding.UTF8.GetBytes(sBodyFrame);
NetMessageData message = new NetMessageData(NetMessageType.BodyData, FrameEncodeType.K4b,
sensorData.depthImageWidth, sensorData.depthImageHeight, lastBodyDataFrameTime);
message.SetData(btBodyData);
bodyDataFrameServer.SendMessageToAllConnections(message);
}
}
// body index frame
if (bodyIndexFrameServer != null && bodyIndexFrameServer.GetConnCount() > 0)
{
if (sensorData.lastBodyIndexFrameTime != lastBodyIndexFrameTime)
{
lastBodyIndexFrameTime = sensorData.lastBodyIndexFrameTime;
NetMessageData message = new NetMessageData(NetMessageType.BodyIndex, FrameEncodeType.Raw,
sensorData.depthImageWidth, sensorData.depthImageHeight, lastBodyIndexFrameTime);
message.SetData(sensorData.bodyIndexImage);
if (compressRawFrames)
{
message.SetCompressor(bodyIndexFrameCompressor);
}
bodyIndexFrameServer.SendMessageToAllConnections(message);
}
}
// pose frame
if (poseFrameServer != null && poseFrameServer.GetConnCount() > 0)
{
if (sensorData.lastSensorPoseFrameTime != lastPoseFrameTime)
{
lastPoseFrameTime = sensorData.lastSensorPoseFrameTime;
string curSensorPoseStr = sensorData.sensorPosePosition.ToString() + " " + sensorData.sensorPoseRotation.eulerAngles.ToString();
if (lastSensorPoseStr != curSensorPoseStr)
{
lastSensorPoseStr = curSensorPoseStr;
KinectInterop.NetPoseData netPoseData = sensorData.sensorInterface.GetSensorNetPoseData(sensorData);
string sPoseData = JsonUtility.ToJson(netPoseData);
byte[] btPoseData = System.Text.Encoding.UTF8.GetBytes(sPoseData);
NetMessageData message = new NetMessageData(NetMessageType.Pose, FrameEncodeType.Json, 0, 0, lastPoseFrameTime);
message.SetData(btPoseData);
poseFrameServer.SendMessageToAllConnections(message);
}
}
}
// depth2color frame
if (depth2colorFrameServer != null && depth2colorFrameServer.GetConnCount() > 0)
{
if (depth2colorTex2d == null)
{
depth2colorTex2d = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight, TextureFormat.ARGB32, false);
sensorData.sensorInterface.EnableDepthCameraColorFrame(sensorData, true);
}
ulong frameTime = 0;
RenderTexture depth2colorTex = sensorData.sensorInterface.GetDepthCameraColorFrameTexture(ref depth2colorTex2d, ref frameTime);
if (depth2colorTex != null && frameTime != lastDepth2ColorFrameTime)
{
lastDepth2ColorFrameTime = frameTime;
byte[] btFrameData = depth2colorTex2d.EncodeToJPG();
NetMessageData message = new NetMessageData(NetMessageType.Depth2Color, FrameEncodeType.Jpeg,
sensorData.depthImageWidth, sensorData.depthImageHeight, frameTime);
message.SetData(btFrameData);
depth2colorFrameServer.SendMessageToAllConnections(message);
}
}
else if (depth2colorTex2d != null && depth2colorFrameServer != null && depth2colorFrameServer.GetConnCount() == 0)
{
// disable depth2color frame
depth2colorTex2d = null;
sensorData.sensorInterface.EnableDepthCameraColorFrame(sensorData, false);
}
// color2depth frame
if (color2depthFrameServer != null && color2depthFrameServer.GetConnCount() > 0)
{
ulong frameTime = 0;
byte[] btFrameData = sensorData.sensorInterface.GetColorCameraDepthFrameBytes(ref frameTime);
if (btFrameData == null)
{
sensorData.sensorInterface.EnableColorCameraDepthFrame(sensorData, true);
color2depthEnabled = true;
}
if (btFrameData != null && frameTime != lastColor2DepthFrameTime)
{
lastColor2DepthFrameTime = frameTime;
NetMessageData message = new NetMessageData(NetMessageType.Color2Depth, FrameEncodeType.Raw,
sensorData.colorImageWidth, sensorData.colorImageHeight, frameTime);
message.SetData(btFrameData);
if (compressRawFrames)
{
message.SetCompressor(color2depthFrameCompressor);
}
color2depthFrameServer.SendMessageToAllConnections(message);
}
}
else if (color2depthEnabled && color2depthFrameServer != null && color2depthFrameServer.GetConnCount() == 0)
{
// disable color2depth frame
color2depthEnabled = false;
sensorData.sensorInterface.EnableColorCameraDepthFrame(sensorData, false);
}
// color2bodyIndex frame
if (color2bodyIndexFrameServer != null && color2bodyIndexFrameServer.GetConnCount() > 0)
{
ulong frameTime = 0;
byte[] btFrameData = sensorData.sensorInterface.GetColorCameraBodyIndexFrame(ref frameTime);
if (btFrameData == null)
{
sensorData.sensorInterface.EnableColorCameraBodyIndexFrame(sensorData, true);
color2bodyIndexEnabled = true;
}
if (btFrameData != null && frameTime != lastColor2BodyIndexFrameTime)
{
lastColor2BodyIndexFrameTime = frameTime;
NetMessageData message = new NetMessageData(NetMessageType.Color2Depth, FrameEncodeType.Raw,
sensorData.colorImageWidth, sensorData.colorImageHeight, frameTime);
message.SetData(btFrameData);
if (compressRawFrames)
{
message.SetCompressor(color2bodyIndexFrameCompressor);
}
color2bodyIndexFrameServer.SendMessageToAllConnections(message);
}
}
else if (color2bodyIndexEnabled && color2bodyIndexFrameServer != null && color2bodyIndexFrameServer.GetConnCount() == 0)
{
// disable color2bodyIndex frame
color2bodyIndexEnabled = false;
sensorData.sensorInterface.EnableColorCameraBodyIndexFrame(sensorData, false);
}
}
catch (System.Exception ex)
{
Debug.LogError("Error updating NetServer: " + ex.Message);
Debug.LogError(ex);
}
}
public ReceivedMessageEventArgs(NetMessageData message)
{
this.message = message;
}
