private void ServerThread()
{
while (running)
{
try
{
TcpClient tcpClient = tcpServer.AcceptTcpClient();
manualController.Enabled = false;
Connected = true;
ClientName = ((IPEndPoint)tcpClient.Client.RemoteEndPoint).Address.ToString();
tcpClient.Client.ReceiveTimeout = 1000;
tcpClient.Client.SendTimeout = 1000;
int zeroCtn = 0;
while (running)
{
try
{
int sz = tcpClient.Client.Receive(receiveBytes);
if (sz == 0)
{
zeroCtn++;
if (zeroCtn == 1000)
{
break;
}
continue;
}
if (receiveBytes[0] == (byte)2 || receiveBytes[0] == (byte)3)
{
continue;
}
int len = BitConverter.ToInt32(receiveBytes, 1);
string name = Encoding.UTF8.GetString(receiveBytes, 5, len);
float value = BitConverter.ToSingle(receiveBytes, 5 + len);
int szHeader = 5 + len + 4;
Debug.Log((receiveBytes[0] == (byte)0 ? "Get: " : "Set: ") + name);
if (receiveBytes[0] == (byte)1)
{
Debug.Log("Value: " + value);
}
float ret = 0;
byte[] retData = null;
if (receiveBytes[0] == (byte)0)
{
if (name == "Time.Wait")
{
Monitor.Enter(m_lock);
frameUpdated = false;
while (!frameUpdated)
{
Monitor.Wait(m_lock);
}
ret = frameTime;
Monitor.Exit(m_lock);
}
else if (name == "Camera.Capture")
{
Monitor.Enter(m_lock);
captureFromCamera = true;
while (captureFromCamera)
{
Monitor.Wait(m_lock);
}
retData = cameraImageBytes;
ret = retData.Length;
Monitor.Exit(m_lock);
}
else if (name == "Lidar.Read")
{
float[] readings = RobotLidar.GetReadings();
retData = new byte[readings.Length * 4];
Buffer.BlockCopy(readings, 0, retData, 0, retData.Length);
ret = readings.Length;
}
else if (name == "Lidar.Std")
{
ret = RobotLidar.Std;
}
else if (name == "World.Camera.Mode")
{
ret = (int)WorldCameraSelector.SelectedCamera;
}
else if (name == "World.Camera.Zoom")
{
ret = WorldCameraSelector.Distance;
}
else if (name == "GPS.X")
{
ret = kinematics.GPS.x;
}
else if (name == "GPS.Y")
{
ret = kinematics.GPS.y;
}
else if (name == "GPS.Theta")
{
ret = kinematics.GPS.z;
}
else if (name == "GPS")
{
retData = new byte[3 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.GPS.x);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.GPS.y);
Array.Copy(bytes, 0, retData, 4, 4);
bytes = BitConverter.GetBytes(kinematics.GPS.z);
Array.Copy(bytes, 0, retData, 8, 4);
ret = 3;
}
else if (name == "GPS.Std.X")
{
ret = kinematics.GPSStd.x;
}
else if (name == "GPS.Std.Y")
{
ret = kinematics.GPSStd.y;
}
else if (name == "GPS.Std.Theta")
{
ret = kinematics.GPSStd.z;
}
else if (name == "GPS.Std")
{
retData = new byte[3 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.GPSStd.x);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.GPSStd.y);
Array.Copy(bytes, 0, retData, 4, 4);
bytes = BitConverter.GetBytes(kinematics.GPSStd.z);
Array.Copy(bytes, 0, retData, 8, 4);
ret = 3;
}
else if (name == "Pose.X")
{
ret = kinematics.Pose.x;
}
else if (name == "Pose.Y")
{
ret = kinematics.Pose.y;
}
else if (name == "Pose.Theta")
{
ret = kinematics.Pose.z;
}
else if (name == "Pose")
{
retData = new byte[3 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.Pose.x);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.Pose.y);
Array.Copy(bytes, 0, retData, 4, 4);
bytes = BitConverter.GetBytes(kinematics.Pose.z);
Array.Copy(bytes, 0, retData, 8, 4);
ret = 3;
}
else if (name == "Velocity.Linear")
{
ret = kinematics.LinearVelocity;
}
else if (name == "Velocity.Angular")
{
ret = kinematics.AngularVelocity;
}
else if (name == "Velocity")
{
retData = new byte[2 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.LinearVelocity);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.AngularVelocity);
Array.Copy(bytes, 0, retData, 4, 4);
ret = 2;
}
else if (name == "Controller.LowLevel")
{
ret = kinematics.LowLevelControl ? 1.0f : 0.0f;
}
else if (name == "Velocity.Angular.Left")
{
ret = kinematics.LeftWheel.AngularSpeed;
}
else if (name == "Velocity.Angular.Right")
{
ret = kinematics.RightWheel.AngularSpeed;
}
else if (name == "Velocity.Wheels")
{
retData = new byte[2 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.LeftWheel.AngularSpeed);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.RightWheel.AngularSpeed);
Array.Copy(bytes, 0, retData, 4, 4);
ret = 2;
}
else if (name == "Odometry.X")
{
ret = kinematics.Odometry.x;
}
else if (name == "Odometry.Y")
{
ret = kinematics.Odometry.y;
}
else if (name == "Odometry.Theta")
{
ret = kinematics.Odometry.z;
}
else if (name == "Odometry")
{
retData = new byte[3 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.Odometry.x);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.Odometry.y);
Array.Copy(bytes, 0, retData, 4, 4);
bytes = BitConverter.GetBytes(kinematics.Odometry.z);
Array.Copy(bytes, 0, retData, 8, 4);
ret = 3;
}
else if (name == "Odometry.Std.Linear")
{
ret = kinematics.MotionStd.x;
}
else if (name == "Odometry.Std.Angular")
{
ret = kinematics.MotionStd.y;
}
else if (name == "Odometry.Std")
{
retData = new byte[2 * 4];
byte[] bytes = BitConverter.GetBytes(kinematics.MotionStd.x);
Array.Copy(bytes, 0, retData, 0, 4);
bytes = BitConverter.GetBytes(kinematics.MotionStd.y);
Array.Copy(bytes, 0, retData, 4, 4);
ret = 2;
}
else if (name == "Trace")
{
ret = kinematics.Trace ? 1.0f : 0.0f;
}
else if (name == "Controller.Manual")
{
ret = manualController.Enabled ? 1.0f : 0.0f;
}
}
else
{
if (name == "Pose.X")
{
kinematics.SetPose(value, kinematics.Pose.y, kinematics.Pose.z);
ret = kinematics.Pose.x;
}
else if (name == "Pose.Y")
{
kinematics.SetPose(kinematics.Pose.x, value, kinematics.Pose.z);
ret = kinematics.Pose.y;
}
else if (name == "Pose.Theta")
{
kinematics.SetPose(kinematics.Pose.x, kinematics.Pose.y, value);
ret = kinematics.Pose.z;
}
else if (name == "Velocity.Linear")
{
kinematics.LinearVelocity = value;
ret = kinematics.LinearVelocity;
}
else if (name == "Velocity.Angular")
{
kinematics.AngularVelocity = value;
ret = kinematics.AngularVelocity;
}
else if (name == "Controller.LowLevel")
{
kinematics.LowLevelControl = value > 0;
ret = kinematics.LowLevelControl ? 1.0f : 0.0f;
}
else if (name == "Velocity.Angular.Left")
{
kinematics.LeftWheel.AngularSpeed = value;
ret = kinematics.LeftWheel.AngularSpeed;
}
else if (name == "Velocity.Angular.Right")
{
kinematics.RightWheel.AngularSpeed = value;
ret = kinematics.RightWheel.AngularSpeed;
}
else if (name == "Odometry.Std.Linear")
{
kinematics.MotionStd.x = value;
ret = kinematics.MotionStd.x;
}
else if (name == "Odometry.Std.Angular")
{
kinematics.MotionStd.y = value;
ret = kinematics.MotionStd.y;
}
else if (name == "Trace")
{
kinematics.Trace = value > 0;
ret = kinematics.Trace ? 1.0f : 0.0f;
}
else if (name == "Lidar.Std")
{
RobotLidar.Std = value;
ret = RobotLidar.Std;
}
else if (name == "GPS.Std.X")
{
kinematics.GPSStd.x = value;
ret = kinematics.GPSStd.x;
}
else if (name == "GPS.Std.Y")
{
kinematics.GPSStd.y = value;
ret = kinematics.GPSStd.y;
}
else if (name == "GPS.Std.Theta")
{
kinematics.GPSStd.z = value;
ret = kinematics.GPSStd.z;
}
else if (name == "World.Camera.Mode")
{
int v = (int)value;
if (v >= 0 && v < 3)
{
WorldCameraSelector.ChooseCamera((CameraSelector.Cameras)v);
}
ret = (int)WorldCameraSelector.SelectedCamera;
}
else if (name == "World.Camera.Zoom")
{
WorldCameraSelector.DistanceChanged(value);
ret = WorldCameraSelector.Distance;
}
else if (name == "GPS.Std.Theta")
{
kinematics.GPSStd.z = value;
ret = kinematics.GPSStd.z;
}
else if (name == "Controller.Manual")
{
manualController.Enabled = value > 0;
ret = manualController.Enabled ? 1.0f : 0.0f;
}
}
byte[] data;
if (retData == null)
{
data = new byte[6];
data[0] = (byte)2;
}
else
{
data = new byte[6 + 4 + retData.Length];
data[0] = (byte)3;
}
data[1] = (byte)1;
byte[] retBytes = BitConverter.GetBytes(ret);
Array.Copy(retBytes, 0, data, 2, 4);
if (retData != null)
{
retBytes = BitConverter.GetBytes(retData.Length);
Array.Copy(retBytes, 0, data, 6, 4);
Array.Copy(retData, 0, data, 10, retData.Length);
}
tcpClient.Client.Send(data);
}
catch (Exception ex)
{
Debug.Log(ex.Message);
}
}
tcpClient.Close();
manualController.Enabled = true;
Connected = false;
ClientName = "";
}
catch (Exception ex)
{
Debug.Log(ex.Message);
}
}
tcpServer.Stop();
Connected = false;
ClientName = "";
}
