private void HandleJsonClient()
{
stream = client.GetStream();
byte[] dataLenBytes = new byte[4];
string jsonFromClient = "";
var sb = new StringBuilder();
RobotController rc = simulationControllerInstance.robotController;
try
{
while (clientConnected)
{
if (stream.DataAvailable)
{
#region JSON_recv
var packetType = (PacketType)ReadByteFromStream(stream);
var packetFlag = (Flag)ReadByteFromStream(stream);
ReadAllFromStream(stream, dataLenBytes, 4);
int dataLength = System.BitConverter.ToInt32(dataLenBytes, 0);
shouldACK = false;
byte[] dataFromClient = ArrayPool <byte> .Shared.Rent(dataLength);
ReadAllFromStream(stream, dataFromClient, dataLength);
jsonFromClient = Encoding.ASCII.GetString(dataFromClient, 0, dataLength);
if (jsonFromClient.Length != dataLength)
{
throw new Exception("wut");
}
if (!packetFlag.HasFlag(Flag.DO_NOT_LOG_PACKET))
{
Debug.Log("Received " + Enum.GetName(typeof(PacketType), packetType) + " len: " + dataLength.ToString());
}
switch (packetType)
{
case PacketType.SET_MTR:
if (!packetFlag.HasFlag(Flag.DO_NOT_LOG_PACKET))
{
Debug.Log("From client: " + jsonFromClient);
}
if (dataLength != 0)
{
var motors = JsonSerializer.Deserialize <JSON.Motors>(jsonFromClient);
rc.SetRawMotors(
motors.FLH,
motors.FLV,
motors.BLV,
motors.BLH,
motors.FRH,
motors.FRV,
motors.BRV,
motors.BRH);
}
else
{
var motors = new JSON.Motors();
motors.FLH = rc.motorFLH.fill;
motors.FLV = rc.motorFLV.fill;
motors.BLV = rc.motorBLV.fill;
motors.BLH = rc.motorBLH.fill;
motors.FRH = rc.motorFRH.fill;
motors.FRV = rc.motorFRV.fill;
motors.BRV = rc.motorBRV.fill;
motors.BRH = rc.motorBRH.fill;
EnqueuePacket(PacketType.SET_MTR, packetFlag, JsonSerializer.ToJsonString(motors));
}
break;
case PacketType.ARM_MTR:
rc.motorsArmed = true;
break;
case PacketType.DISARM_MTR:
rc.motorsArmed = false;
break;
case PacketType.SET_CONTROL_MODE:
if (dataLength != 0)
{
rc.motorsControlMode = jsonFromClient;
}
else
{
EnqueuePacket(PacketType.SET_CONTROL_MODE, packetFlag, rc.motorsControlMode);
}
break;
case PacketType.SET_ACRO:
if (dataLength != 0)
{
var acro = JsonSerializer.Deserialize <JSON.AcroOptions>(jsonFromClient);
rc.targetRotationSpeed.x = acro.rotSpeed.x;
rc.targetRotationSpeed.y = acro.rotSpeed.y;
rc.targetRotationSpeed.z = acro.rotSpeed.z;
rc.velocity.x = acro.vel.x;
rc.velocity.y = acro.vel.y;
rc.velocity.z = acro.vel.z;
}
else
{
var acro = new JSON.AcroOptions();
acro.rotSpeed.x = rc.targetRotationSpeed.x;
acro.rotSpeed.y = rc.targetRotationSpeed.y;
acro.rotSpeed.z = rc.targetRotationSpeed.z;
acro.vel.x = rc.velocity.x;
acro.vel.y = rc.velocity.y;
acro.vel.z = rc.velocity.z;
EnqueuePacket(PacketType.SET_ACRO, packetFlag, JsonSerializer.ToJsonString(acro));
}
break;
case PacketType.SET_STABLE:
if (dataLength != 0)
{
var stable = JsonSerializer.Deserialize <JSON.StableOptions>(jsonFromClient);
rc.targetRotation.x = stable.rot.x;
rc.targetRotation.y = stable.rot.y;
rc.targetRotation.z = stable.rot.z;
rc.velocity.x = stable.vel.x;
rc.velocity.y = stable.vel.y;
rc.velocity.z = stable.vel.z;
rc.targetDepth = stable.depth;
}
else
{
var stable = new JSON.StableOptions();
stable.rot.x = rc.targetRotation.x;
stable.rot.y = rc.targetRotation.y;
stable.rot.z = rc.targetRotation.z;
stable.vel.x = rc.velocity.x;
stable.vel.y = rc.velocity.y;
stable.vel.z = rc.velocity.z;
stable.depth = rc.targetDepth;
EnqueuePacket(PacketType.SET_STABLE, packetFlag, JsonSerializer.ToJsonString(stable));
}
break;
case PacketType.SET_PID:
if (dataLength != 0)
{
var pids = JsonSerializer.Deserialize <JSON.PIDs>(jsonFromClient);
rc.rollPID.SetValues(pids.roll);
rc.pitchPID.SetValues(pids.pitch);
rc.yawPID.SetValues(pids.yaw);
rc.depthPID.SetValues(pids.depth);
}
else
{
var pids = new JSON.PIDs();
rc.rollPID.GetValues(ref pids.roll);
rc.pitchPID.GetValues(ref pids.pitch);
rc.yawPID.GetValues(ref pids.yaw);
rc.depthPID.GetValues(ref pids.depth);
EnqueuePacket(PacketType.SET_PID, packetFlag, JsonSerializer.ToJsonString <JSON.PIDs>(pids));
}
break;
case PacketType.GET_SENS:
EnqueuePacket(PacketType.GET_SENS, packetFlag, JsonSerializer.ToJsonString(rc.allSensors.Get()));
break;
case PacketType.GET_DEPTH:
MainThreadUpdateWorker depthWorker = new MainThreadUpdateWorker()
{
action = () => {
byte[] map = simulationControllerInstance.GetDepthMap();
EnqueuePacket(PacketType.GET_DEPTH, packetFlag, System.Convert.ToBase64String(map));
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(depthWorker);
break;
case PacketType.GET_DEPTH_BYTES:
depthWorker = new MainThreadUpdateWorker()
{
action = () => {
byte[] map = simulationControllerInstance.GetDepthMap();
EnqueuePacket(PacketType.GET_DEPTH_BYTES, packetFlag, map, map.Length, false);
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(depthWorker);
break;
case PacketType.GET_VIDEO_BYTES:
depthWorker = new MainThreadUpdateWorker()
{
action = () =>
{
byte[] map = simulationControllerInstance.GetVideo();
EnqueuePacket(PacketType.GET_VIDEO_BYTES, packetFlag, map, map.Length, false);
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(depthWorker);
break;
case PacketType.GET_VIDEO:
MainThreadUpdateWorker videoWorker = new MainThreadUpdateWorker()
{
action = () => {
byte[] vid = simulationControllerInstance.GetVideo();
EnqueuePacket(PacketType.GET_VIDEO, packetFlag, System.Convert.ToBase64String(vid));
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(videoWorker);
break;
case PacketType.ACK:
EnqueuePacket(PacketType.ACK, packetFlag | Flag.TEST, "{\"info\":\"ack ack\"}");
break;
case PacketType.SET_ORIEN:
if (dataLength != 0)
{
rc.orientation.Set(JsonSerializer.Deserialize <JSON.Orientation>(jsonFromClient));
}
else
{
EnqueuePacket(PacketType.SET_ORIEN, packetFlag, JsonSerializer.ToJsonString(rc.orientation.Get()));
}
break;
case PacketType.RST_SIM:
MainThreadUpdateWorker resetWorker = new MainThreadUpdateWorker()
{
action = () => { GameObject.FindGameObjectWithTag("SimulationController").GetComponent <SimulationController>().PlaceRobotInStartZone(); }
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(resetWorker);
simulationControllerInstance.SendToClients(PacketType.RST_SIM, Flag.None);
break;
case PacketType.PING:
EnqueuePacket(PacketType.PING, packetFlag, jsonFromClient);
break;
case PacketType.GET_CPS:
MainThreadUpdateWorker checkpointWorker = new MainThreadUpdateWorker()
{
action = () => {
string ret = "[";
foreach (var obj in GameObject.FindGameObjectsWithTag("Checkpoint"))
{
ret += "{\"id\":\"" + obj.GetComponent <CheckpointController>().id + "\",\"reached\":" + obj.GetComponent <CheckpointController>().reached.ToString().ToLower() + "},";
}
ret += "]";
ret = ret.Replace(",]", "]");
EnqueuePacket(PacketType.GET_CPS, packetFlag, ret);
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(checkpointWorker);
break;
case PacketType.CHK_AP:
string id = jsonFromClient;
var actionpointWorker = new MainThreadUpdateWorker()
{
action = () =>
{
if (id.Length != 0)
{
bool ret = false;
foreach (GameObject obj in GameObject.FindGameObjectsWithTag("Actionpoint"))
{
ActionpointController apc = obj.GetComponent <ActionpointController>();
if (apc.id.Equals(id, StringComparison.Ordinal))
{
ret = apc.active;
}
}
if (ret)
{
EnqueuePacket(PacketType.CHK_AP, Flag.None, "true");
}
else
{
EnqueuePacket(PacketType.CHK_AP, Flag.None, "false");
}
}
else
{
sb.Clear();
sb.Append('[');
foreach (GameObject obj in GameObject.FindGameObjectsWithTag("Actionpoint"))
{
ActionpointController apc = obj.GetComponent <ActionpointController>();
sb.Append(apc.id);
sb.Append(",");
}
if (sb[sb.Length - 1] == ',')
{
sb[sb.Length - 1] = ']';
}
else
{
sb.Append("]");
}
EnqueuePacket(PacketType.CHK_AP, Flag.None, sb.ToString());
}
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(actionpointWorker);
break;
case PacketType.GET_DETE:
MainThreadUpdateWorker detectionWorker = new MainThreadUpdateWorker()
{
action = () => {
var detection = simulationControllerInstance.GetDetection();
EnqueuePacket(PacketType.GET_DETE, packetFlag, JsonSerializer.ToJsonString(detection));
}
};
simulationControllerInstance.mainThreadUpdateWorkers.Enqueue(detectionWorker);
break;
case (PacketType)0xFF:
Debug.LogWarning("got illegal 0xFF packet type");
clientConnected = false;
break;
default:
Debug.LogWarning("Unknown dataframe type " + System.BitConverter.ToString(new byte[] { (byte)packetType }));
EnqueuePacket(PacketType.ERROR, Flag.None, "{\"info\":\"Something went wrong. You shouldn't get this packet. Unknown dataframe packet!\", \"fromClient\":\"" + jsonFromClient + "\"}");
break;
}
if (packetFlag.HasFlag(Flag.SERVER_ECHO))
{
EnqueuePacket(PacketType.ACK, Flag.None, "{\"fromClient\":\"" + jsonFromClient + "\"}");
}
ArrayPool <byte> .Shared.Return(dataFromClient);
#endregion
}
else
{
Thread.Sleep(1);
}
while (!toSend.IsEmpty)
{
if (toSend.TryDequeue(out Packet packet))
{
Send(packet.packetType, packet.flag, packet.bytes, packet.length);
if (packet.rented)
{
ArrayPool <byte> .Shared.Return(packet.bytes);
}
}
}
}
}
catch (Exception exp)
{
Debug.LogError("Json client exception\n" + jsonFromClient + "\n" + exp.Message + '\n' + exp.StackTrace);
clientConnected = false;
}
Stop();
}
