// Get Object Pose
private void Command_3(byte[] recv, RobotConnection conn)
{
int obj = BitConverter.ToInt32(recv, 1);
obj = IPAddress.NetworkToHostOrder(obj);
int[] pose = SimManager.instance.GetObjectPoseByID(obj);
Packet p = new Packet();
p.packetType = PacketType.SERVER_MESSAGE;
p.dataSize = sizeof(int) * 3 + 1;
p.data = new byte[p.dataSize];
if (pose == null)
{
Array.Clear(p.data, 0, 13);
}
else
{
p.data[0] = 1;
BitConverter.GetBytes(IPAddress.HostToNetworkOrder(pose[0])).CopyTo(p.data, 1);
BitConverter.GetBytes(IPAddress.HostToNetworkOrder(pose[1])).CopyTo(p.data, 5);
BitConverter.GetBytes(IPAddress.HostToNetworkOrder((360 - pose[2]) % 360)).CopyTo(p.data, 9);
}
serverManager.WritePacket(conn, p);
}
// Play Beep
private void Command_b(byte[] recv, RobotConnection conn)
{
if (conn.robot is IAudio)
{
(conn.robot as IAudio).PlayBeep();
}
}
// Accept a pending connection
private void AcceptConnection()
{
// Check if there is a robot ready to receive control
if (activeRobot == null)
{
Debug.Log("Control program connected but no robot active");
EyesimLogger.instance.Log("Server: Received connection request with no active robot");
TcpClient client = listener.AcceptTcpClient();
RobotConnection newClient = new RobotConnection(client, -1);
RejectConnection(newClient);
return;
}
else
{
// Create a new TcpClient to receive messages
TcpClient client = listener.AcceptTcpClient();
RobotConnection newClient = new RobotConnection(client, robotIDs);
// Associate the RobotConnection
newClient.robot = activeRobot;
activeRobot.myConnection = newClient;
conns.Add(newClient);
robotIDs++;
// Reply to the robot to begin control
Debug.Log("Accepted a connection");
EyesimLogger.instance.Log("Server: Connection accepted. Executing on robot ID " + activeRobot.objectID);
ReplyHandshake(newClient);
connectionReceived = true;
}
}
} //constructor (private becouse can be created only once)
private void Init()
{
robotConnection = new RobotConnection();
visionParam = new VisionParam();
calculate = new Calculate();
mainCycle = new Stahli2Robots.MainCycle();
errorDescription = new ErrorDescription();
////////////////////////////
loadCarrier = new Carrier();
loadTray = new Tray();
unLoadTray = new Tray(); //no camera
unLoadCarrier = new Tray();
cam1 = new VisionState();
cam2 = new VisionState();
cam3 = new VisionState();
////////////////////////////
xmlSerialize = new Stahli2Robots.XMLSerialize();
appSetting = new AppSettings();
orderParams = new OrderParams();
appSetting.Init();
orderParams.Init();
}
public void ReturnDriveDone(RobotConnection conn)
{
Packet p = new Packet();
p.packetType = PacketType.DRIVE_DONE;
p.dataSize = 0;
serverManager.WritePacket(conn, p);
}
// Terminate a connection, and remove form the connection list
private void CloseConnection(RobotConnection conn)
{
conn.tcpClient.Close();
if (!conns.Remove(conn))
{
Debug.Log("Failed to remove connection");
}
}
// Set Robot Pose
private void Command_2(byte[] recv, RobotConnection conn)
{
int xPos = IPAddress.NetworkToHostOrder(BitConverter.ToInt32(recv, 1));
int yPos = IPAddress.NetworkToHostOrder(BitConverter.ToInt32(recv, 5));
int phi = IPAddress.NetworkToHostOrder(BitConverter.ToInt32(recv, 9));
conn.robot.SetRobotPosition(xPos, yPos, phi);
}
// Set Speed
private void Command_x(byte[] recv, RobotConnection conn)
{
if (conn.robot is IVWDrive)
{
int linSpeed = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 1));
int angSpeed = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 3));
(conn.robot as IVWDrive).VWSetVehicleSpeed(linSpeed, angSpeed);
}
}
// VW Drive Turn
private void Command_Y(byte[] recv, RobotConnection conn)
{
if (conn.robot is IVWDrivable)
{
int velocity = IPAddress.HostToNetworkOrder(BitConverter.ToInt16(recv, 1));
int angle = IPAddress.HostToNetworkOrder(BitConverter.ToInt16(recv, 3));
(conn.robot as IVWDrivable).VWDriveTurn(angle, velocity);
}
}
public void ReturnDriveDone(RobotConnection conn)
{
Packet p = new Packet();
p.packetType = PacketType.DRIVE_DONE;
p.dataSize = 0;
Debug.Log("writing back drive done!");
serverManager.WritePacket(conn, p);
}
// Set Camera
private void Command_F(byte[] recv, RobotConnection conn)
{
if (conn.robot is ICameras)
{
int camera = recv[1] - 1;
int width = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 2));
int height = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 4));
(conn.robot as ICameras).SetCameraResolution(camera, width, height);
}
}
public void ForwardRadioMessage(RobotConnection conn, byte[] msg)
{
Debug.Log("Forwarding a Packet");
Packet p = new Packet();
p.packetType = PacketType.RADIO_MESSAGE;
p.dataSize = msg.Length;
p.data = msg;
serverManager.WritePacket(conn, p);
}
// VW Drive Curve
private void Command_C(byte[] recv, RobotConnection conn)
{
if (conn.robot is IVWDrivable)
{
int speed = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 1));
int distance = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 3));
int angle = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 5));
(conn.robot as IVWDrivable).VWDriveCurve(distance, angle, speed);
}
}
//Drive Wait
private void Command_L(byte[] recv, RobotConnection conn)
{
if (conn.robot is IVWDrive)
{
(conn.robot as IVWDrive).VWDriveWait(ReturnDriveDone);
}
else
{
Debug.Log("Requested drive wait from a non VW drivable robot");
}
}
// VW Drive Straight
private void Command_y(byte[] recv, RobotConnection conn)
{
if (conn.robot is IVWDrivable)
{
// Velocity is first byte, distance is second byte
// Order revered in input array to match function call semantics
int distance = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 3));
int speed = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 1));
(conn.robot as IVWDrivable).VWDriveStraight(distance, speed);
}
}
// Reject a connection (due to no robot present)
private void RejectConnection(RobotConnection conn)
{
Packet p = new Packet();
p.packetType = PacketType.SERVER_DISCONNECT;
p.dataSize = 18;
p.data = new byte[18];
System.Text.Encoding.ASCII.GetBytes("No robot in scene").CopyTo(p.data, 0);
p.data[17] = 0;
WritePacket(conn, p);
}
// Read a packet from a connection
// DataAvailable flag must be true before calling
private void ReadPacket(RobotConnection conn)
{
NetworkStream stream = conn.tcpClient.GetStream();
// Read Header
int bytesRead = stream.Read(recvBuf, 0, 5);
// Check the read is successful
if (bytesRead != 5)
{
// If failed, flush the read buffer
Debug.Log("Failed to read packet header");
EyesimLogger.instance.Log("Server: Packet read failure");
while (stream.DataAvailable)
{
stream.Read(recvBuf, 0, recvBuf.Length);
}
return;
}
uint dataSize = BitConverter.ToUInt32(recvBuf, 1);
if (BitConverter.IsLittleEndian)
{
dataSize = RobotFunction.ReverseBytes(dataSize);
}
int packetType = recvBuf[0];
// Read Body
if (dataSize > 0)
{
bytesRead = stream.Read(recvBuf, 0, (int)dataSize);
}
switch (packetType)
{
case PacketType.CLIENT_HANDSHAKE:
if (conn.robot == null)
{
}
break;
case PacketType.CLIENT_MESSAGE:
interpreter.ReceiveCommand(recvBuf, conn);
break;
case PacketType.CLIENT_DISCONNECT:
Debug.Log("Client requested disconnect");
CloseConnection(conn);
break;
default:
break;
}
}
// Get Vehicle Pose
private void Command_q(byte[] recv, RobotConnection conn)
{
if (conn.robot is IPosable)
{
Pose pose = (conn.robot as IPosable).GetPose();
Debug.Log("x: " + pose.x + " y: " + pose.y + " phi: " + pose.phi);
}
else
{
Debug.Log("Requested pose from a non posable robot");
}
}
// Accept a pending connection
private void AcceptConnection()
{
TcpClient client = listener.AcceptTcpClient();
RobotConnection newClient = new RobotConnection(client, robotIDs);
newClient.robot = testBot;
testBot.myConnection = newClient;
conns.Add(newClient);
robotIDs++;
Debug.Log("Accepted a connection");
ReplyHandshake(newClient);
}
// Drive Remaining
private void Command_z(byte[] recv, RobotConnection conn)
{
if (conn.robot is IVWDrivable)
{
bool done = (conn.robot as IVWDrivable).VWDriveDone();
Packet p = new Packet();
p.packetType = PacketType.SERVER_MESSAGE;
p.dataSize = 1;
p.data = BitConverter.GetBytes(done);
serverManager.WritePacket(conn, p);
}
}
// Get Camera
private void Command_f(byte[] recv, RobotConnection conn)
{
if (conn.robot is ICameras)
{
byte[] img = (conn.robot as ICameras).GetCameraOutput(0);
Packet packet = new Packet();
packet.packetType = PacketType.SERVER_CAMIMG;
packet.dataSize = (UInt32)img.Length;
packet.data = img;
serverManager.WritePacket(conn, packet);
}
}
// Get Encoder
private void Command_e(byte[] recv, RobotConnection conn)
{
if (conn.robot is IMotors)
{
int quad = recv[1] - 1;
int tick = (conn.robot as IMotors).GetEncoder(quad);
Packet p = new Packet();
p.packetType = PacketType.SERVER_MESSAGE;
p.dataSize = 4;
p.data = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(tick));
serverManager.WritePacket(conn, p);
}
}
// Handshake connection with control program
private void ReplyHandshake(RobotConnection conn)
{
Packet p = new Packet();
p.packetType = PacketType.SERVER_HANDSHAKE;
p.dataSize = 0;
WritePacket(conn, p);
p = new Packet();
p.packetType = PacketType.SERVER_READY;
p.dataSize = 0;
WritePacket(conn, p);
}
public UBTController(DelegateUpdateInfo fxUpdateInfo, DelegateRefreshServo fxRefreshServo, RobotConnection robot)
{
updateInfo = fxUpdateInfo;
refreshServo = fxRefreshServo;
this.robot = robot;
InitServo();
// InitSerialPort();
config = new data.BoardConfig();
networkConfig = new data.NetworkConfig();
comboTable = new data.ComboTable();
actionTable = new data.ActionTable();
}
// Set Servo position
private void Command_s(byte[] recv, RobotConnection conn)
{
if (conn.robot is IServoSettable)
{
int servo = recv[1] - 1;
int angle = recv[2];
(conn.robot as IServoSettable).SetServo(servo, angle);
}
else
{
Debug.Log("Set Servo Command received for a non servo settable robot");
}
}
// Motor Drive Controlled
private void Command_M(byte[] recv, RobotConnection conn)
{
if (conn.robot is IPIDUsable)
{
int motor = recv[1] - 1;
int ticks = recv[2];
(conn.robot as IPIDUsable).DriveMotorControlled(motor, ticks);
}
else
{
Debug.Log("Drive Command received for a non drivable robot");
}
}
// Motor Drive Uncontrolled
private void Command_m(byte[] recv, RobotConnection conn)
{
if (conn.robot is IMotors)
{
int motor = recv[1] - 1;
int speed = recv[2];
(conn.robot as IMotors).DriveMotor(motor, speed);
}
else
{
Debug.Log("Drive Command received for a non drivable robot");
}
}
// Drive Motor Uncontrolled
private void Command_m(byte[] recv, RobotConnection conn)
{
if (conn.robot is IMotors)
{
int motor = recv[1] - 1;
int speed = (SByte)recv[2];
speed = (int)Mathf.Clamp(speed, -100, 100);
(conn.robot as IMotors).DriveMotor(motor, speed);
}
else
{
Debug.Log("Drive Command received for a non drivable robot");
}
}
// Get own ID
private void Command_i(byte[] recv, RobotConnection conn)
{
int id = conn.robot.objectID;
id = IPAddress.HostToNetworkOrder(id);
Packet p = new Packet();
p.packetType = PacketType.SERVER_MESSAGE;
p.dataSize = 4;
p.data = new byte[p.dataSize];
BitConverter.GetBytes(id).CopyTo(p.data, 0);
serverManager.WritePacket(conn, p);
}
// Get Pose
private void Command_Q(byte[] recv, RobotConnection conn)
{
if (conn.robot is IPosable)
{
int x = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 1));
int y = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 3));
int phi = IPAddress.NetworkToHostOrder(BitConverter.ToInt16(recv, 5));
(conn.robot as IPosable).SetPose(x, y, phi);
}
else
{
Debug.Log("Requested pose from a non posable robot");
}
}
