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(); }