public void ReceiveGetCarteisanPositions(NetworkMessage message)
{
float[] positions = KinovaAPI.GetCartesianPositions();
Debug.Log("<color=blue>Received:</color> got cart positions:"
+ positions[0] + ", "
+ positions[1] + ", "
+ positions[2] + ", "
+ positions[3] + ", "
+ positions[4] + ", "
+ positions[5] + ", "
+ positions[6] + ", "
+ positions[7]
);
//if (clientView == null) {
string currentArmCartesianPositionStr =
positions[0].ToString() + "," + // x pos
positions[1].ToString() + "," +
positions[2].ToString() + "," +
positions[3].ToString() + "," + // x rot
positions[4].ToString() + "," +
positions[5].ToString() + "," +
positions[6].ToString() + "," + // finger 1
positions[7].ToString() + "," +
0.ToString(); // no 3rd finger, so placing a zero..
//FindObjectOfType<ClientBroadcaster>().SendPosToClient(currentArmCartesianPositionStr);
//}
}
// Create a client and connect to the server port
// public void SetupClient ()
// {
//myClient = new NetworkClient ();
//InitClient ();
//myClient.Connect (address, port);
//Debug.Log ("Started client");
// }
// // Create a local client and connect to the local server
// public void SetupLocalClient ()
// {
//myClient = ClientScene.ConnectLocalServer ();
//InitClient ();
//videoChat.remoteView.GetComponent<CameraController>().StartLocalStream();
//Debug.Log ("Started local client");
//}
//private void InitClient ()
//{
// myClient.RegisterHandler (MsgType.Connect, OnConnected);
// cameraRig.SetActive (true); // transitively enables VIVE controllers
// if (!localRun) {
// videoChat.gameObject.SetActive (true);
// }
// isAtStartup = false;
//}
// // Client function
// public void OnConnected (NetworkMessage netMsg)
// {
//Debug.Log ("Connected to server on " + address + ":" + port);
//if (!localRun) {
// Invoke ("JoinVideoChat", 3.0f);
//}
//connectedToServer = true;
// }
// private void JoinVideoChat ()
// {
//videoChat.JoinVideoChat ();
// }
//shawn test
// public void SendMoveArmCartesianPosition_MoveRelativeMessage(bool rightArm, float X, float Y, float Z, float ThetaX, float ThetaY, float ThetaZ)
//{
// if (!connectedToServer)
// {
// Debug.LogWarning("Not connected to server!");
// return;
// }
// Debug.Log("Sending move " + ArmSide(rightArm) + "with MoveArmCartesianPositionMoveRelative sent!");
// MoveArmCartesianPosition_MoveRelativeMessage m = new MoveArmCartesianPosition_MoveRelativeMessage();
// m.rightArm = rightArm;
// m.X = X;
// m.Y = Y;
// m.Z = Z;
// m.ThetaX = ThetaX;
// m.ThetaY = ThetaY;
// m.ThetaZ = ThetaZ;
// myClient.Send(MyMsgTypes.MSG_MOVE_ARM_ANGULAR_POSITION, m);
//}
public void ReceiveMoveArmWithFingers(NetworkMessage message)
{
MoveArmPositionWithFingersMessage m = message.ReadMessage <MoveArmPositionWithFingersMessage>();
KinovaAPI.MoveArmCartesianPositionWithFingers(m.rightArm, m.x, m.y, m.z, m.thetaX, m.thetaY, m.thetaZ, m.fp1, m.fp2, m.fp3);
hud.armPosition.text = m.x.ToString("0.00") + "," + m.y.ToString("0.00") + "," + "," + m.z.ToString("0.00") + ": rot:" + m.thetaX.ToString("0.00") + "," + m.thetaY.ToString("0.00") + "," + m.thetaZ.ToString("0.00");
}
private void ReceiveMoveFingers(NetworkMessage message)
{
MoveFingersMessage m = message.ReadMessage <MoveFingersMessage> ();
Debug.Log("Move " + ArmSide(m.rightArm) + " arm fingers received!");
KinovaAPI.MoveFingers(m.rightArm, m.pinky, m.ring, m.middle, m.index, m.thumb);
}
private void ReceiveMoveArmHome(NetworkMessage message)
{
MoveArmHomeMessage m = message.ReadMessage <MoveArmHomeMessage> ();
Debug.Log("Stop " + ArmSide(m.rightArm) + " arm received!");
KinovaAPI.MoveArmHome(m.rightArm);
}
private void ReceiveMoveArmNoThetaY(NetworkMessage message)
{
MoveArmNoThetaYMessage m = message.ReadMessage <MoveArmNoThetaYMessage>();
Debug.Log("Move " + ArmSide(m.rightArm) + " arm received!");
KinovaAPI.MoveHandNoThetaY(m.rightArm, m.x, m.y, m.z, m.thetaX, m.thetaZ);
}
//public void RecieveMoveArms_WithFingers(NetworkMessage message)
//{
// MoveArmWithFingersMessage m = message.ReadMessage<MoveArmWithFingersMessage>();
// // Debug.LogError("Move " + ArmSide(m.rightArm) + " with MoveArmCartesianPositionWithFingers received!");
// KinovaAPI.MoveArmCartesianPositionWithFingers(m.rightArm, m.x, m.y, m.z, m.thetaX, m.thetaY, m.thetaZ, m.fp1, m.fp2, m.fp3);
// Debug.Log("receiving:" + m.x + "," + m.y + "," + m.z + "," + m.thetaX + "," + m.thetaY + "," + m.thetaZ + ", fingers: " + m.fp1 + "," + m.fp2 + "," + m.fp3);
//}
//public void RecieveMoveArmCartesianPosition_MoveRelative(NetworkMessage message)
//{
// //MoveArmAngularPositionMessage m = message.ReadMessage<MoveArmAngularPositionMessage>();
// MoveArmCartesianPosition_MoveRelativeMessage m = message.ReadMessage<MoveArmCartesianPosition_MoveRelativeMessage>();
// Debug.LogError("Move " + ArmSide(m.rightArm) + " with MoveArmCartesianPosition_MoveRelativeMessage received!");
// KinovaAPI.MoveArmCartesianPositionRelative(m.rightArm, m.X, m.Y, m.Z, m.ThetaX, m.ThetaY, m.ThetaZ);
//}
//public void SendMoveArmAngularVelocity(bool rightArm, float av1, float av2, float av3, float av4, float av5, float av6, float av7)
//{
// if (!connectedToServer)
// {
// //Debug.LogWarning ("Not connected to server!");
// return;
// }
// Debug.Log("Sending move " + ArmSide(rightArm) + " arm...");
// MoveArmAngularVelocityMessage m = new MoveArmAngularVelocityMessage();
// m.rightArm = rightArm;
// m.av1 = av1;
// m.av2 = av2;
// m.av3 = av3;
// m.av4 = av4;
// m.av5 = av5;
// m.av6 = av6;
// m.av7 = av7;
// myClient.Send(MyMsgTypes.MSG_MOVE_ARM_ANGULAR_VELOCITY, m);
//}
//public void RecieveMoveArmAngularVelocity(NetworkMessage message)
//{
// MoveArmAngularVelocityMessage m = message.ReadMessage<MoveArmAngularVelocityMessage>();
// Debug.LogError("Move " + ArmSide(m.rightArm) + " with MoveArmAngularVelocity received!");
// KinovaAPI.MoveArmAngularVelocity(m.rightArm, m.av1, m.av2, m.av3, m.av4, m.av5, m.av6, m.av7);
//}
//public void ReceieveMoveArmAngularVelocityLooped(NetworkMessage message)
//{
// MoveArmAngularVelocityLoopedMessage m = message.ReadMessage<MoveArmAngularVelocityLoopedMessage>();
// Debug.LogError("Move " + ArmSide(m.rightArm) + " with MoveArmAngularVelocityLooped received!");
// KinovaAPI.MoveArmAngularVelocityLooped(m.rightArm, m.iterations, m.av1, m.av2, m.av3, m.av4, m.av5, m.av6, m.av7);
//}
//public void SendMoveArmAngularPosition(bool rightArm, int ap1,int ap2, int ap3, int ap4, int ap5, int ap6, int ap7)
//{
// if (!connectedToServer)
// {
// //Debug.LogWarning ("Not connected to server!");
// return;
// }
// Debug.Log("Sending move " + ArmSide(rightArm) + "with MoveArmAngularPosition recieved!");
// MoveArmAngularPositionMessage m = new MoveArmAngularPositionMessage();
// m.rightArm = rightArm;
// m.ap1 = ap1;
// m.ap2 = ap2;
// m.ap3 = ap3;
// m.ap4 = ap4;
// m.ap5 = ap5;
// m.ap6 = ap6;
// m.ap7 = ap7;
// myClient.Send(MyMsgTypes.MSG_MOVE_ARM_ANGULAR_POSITION, m);
//}
// public void RecieveMoveArmAngularPosition(NetworkMessage message)
// {
// MoveArmAngularPositionMessage m = message.ReadMessage<MoveArmAngularPositionMessage>();
// Debug.LogError("Move " + ArmSide(m.rightArm) + " with MoveArmAngularPostion received!");
// KinovaAPI.MoveArmAngularPosition(m.rightArm, m.ap1, m.ap2, m.ap3, m.ap4, m.ap5, m.ap6, m.ap7);
// }
// // end shawn test 10.1.17
// public void SendMoveArm (bool rightArm, float x, float y, float z, float thetaX, float thetaY, float thetaZ)
// {
//if (!connectedToServer) {
// //Debug.LogWarning ("Not connected to server!");
// return;
//}
//Debug.Log ("Sending move " + ArmSide(rightArm) + " arm...");
// MoveArmMessage m = new MoveArmMessage();
// m.rightArm = rightArm;
// m.x = x;
// m.y = y;
// m.z = z;
// m.thetaX = thetaX;
// m.thetaY = thetaY;
// m.thetaZ = thetaZ;
// myClient.Send (MyMsgTypes.MSG_MOVE_ARM, m);
// }
// private void ReceiveMoveArm (NetworkMessage message)
// {
//MoveArmMessage m = message.ReadMessage<MoveArmMessage>();
//Debug.LogError ("Move " + ArmSide(m.rightArm) + " arm received!");
// KinovaAPI.MoveHand(m.rightArm, m.x, m.y, m.z, m.thetaX, m.thetaY, m.thetaZ);
// Debug.LogError(m.rightArm + " " + m.x + " " + m.y + " " + m.z + " " + m.thetaX + " " + m.thetaY + " " + m.thetaZ);
// }
// public void SendMoveArmNoThetaY (bool rightArm, float x, float y, float z, float thetaX, float thetaZ)
// {
//if (!connectedToServer) {
// //Debug.LogWarning ("Not connected to server!");
// return;
//}
//Debug.Log ("Sending move " + ArmSide(rightArm) + " arm no theta y...");
//MoveArmNoThetaYMessage m = new MoveArmNoThetaYMessage();
// m.rightArm = rightArm;
// m.x = x;
// m.y = y;
// m.z = z;
// m.thetaX = thetaX;
// m.thetaZ = thetaZ;
// myClient.Send (MyMsgTypes.MSG_MOVE_ARM_NO_THETAY, m);
// }
// private void ReceiveMoveArmNoThetaY (NetworkMessage message)
// {
//MoveArmNoThetaYMessage m = message.ReadMessage<MoveArmNoThetaYMessage>();
//Debug.Log ("Move " + ArmSide(m.rightArm) + " arm received!");
// KinovaAPI.MoveHandNoThetaY(m.rightArm, m.x, m.y, m.z, m.thetaX, m.thetaZ);
// }
// public void SendMoveArmHome (bool rightArm)
// {
//if (!connectedToServer) {
// // Debug.LogWarning ("Not connected to server!");
// return;
//}
//Debug.Log ("Sending move " + ArmSide (rightArm) + " arm home...");
//MoveArmHomeMessage m = new MoveArmHomeMessage();
// m.rightArm = rightArm;
// myClient.Send (MyMsgTypes.MSG_MOVE_ARM_HOME, m);
// }
// private void ReceiveMoveArmHome (NetworkMessage message)
// {
//MoveArmHomeMessage m = message.ReadMessage<MoveArmHomeMessage> ();
//Debug.Log ("Stop " + ArmSide (m.rightArm) + " arm received!");
// KinovaAPI.MoveArmHome(m.rightArm);
// }
//public void SendStopArm (bool rightArm, bool suppressLog)
//{
// if (!connectedToServer) {
// // Debug.LogWarning ("Not connected to server!");
// return;
// }
// if (!suppressLog) {
// Debug.Log ("Sending stop " + ArmSide (rightArm) + " arm...");
// }
// StopArmMessage m = new StopArmMessage();
// m.rightArm = rightArm;
// m.suppressLog = suppressLog;
// myClient.Send (MyMsgTypes.MSG_STOP_ARM, m);
//}
private void ReceiveStopArm(NetworkMessage message)
{
StopArmMessage m = message.ReadMessage <StopArmMessage> ();
// Debug.Log("<color=blue>Received:</color><color=red> stop arm at </color>" + Time.time);
KinovaAPI.StopArm(m.rightArm);
hud.stopReceived.text = Time.time.ToString();
}
public void ReceiveMoveArmUpdate(NetworkMessage message)
{
MoveArmUpdateMessage m = message.ReadMessage <MoveArmUpdateMessage>();
KinovaAPI.MoveArmUpdate();
hud.updateReceived.text = Time.time.ToString();
//Debug.Log("<color=blue>Received:</color> Update arm pos");
}
public void ReceiveSetArmPosition(NetworkMessage message)
{
SetArmPositionMessage m = message.ReadMessage <SetArmPositionMessage>();
KinovaAPI.SetArmPosition(m.rightArm, m.x, m.y, m.z, m.thetaX, m.thetaY, m.thetaZ);
hud.armPosition.text = m.x + ", " + m.y + ", " + m.z + ", " + m.thetaX + ", " + m.thetaY + ", " + m.thetaZ;
// Debug.Log("<color=blue>Received:</color> set arm pos mess:" + m.x + ","+m.y+","+m.z);
}
public void ReceiveSetFingerPosition(NetworkMessage message)
{
SetFingerPositionMessage m = message.ReadMessage <SetFingerPositionMessage>();
//KinovaAPI.SetFingerPosition(m.rightArm,m.fp1,m.fp2,m.fp3);
KinovaAPI.SetFingerPosition(m.rightArm, m.fp1, m.fp2, m.fp3);
hud.fingerPosition.text = m.fp1 + "," + m.fp2 + "," + m.fp3;
//Debug.Log("<color=blue>Received:</color> set arm finger mess:" + m.fp1+","+m.fp2+","+m.fp3);
}
private void ReceiveStopArm(NetworkMessage message)
{
StopArmMessage m = message.ReadMessage <StopArmMessage> ();
if (!m.suppressLog)
{
Debug.Log("Stop " + ArmSide(m.rightArm) + " arm received!");
}
KinovaAPI.StopArm(m.rightArm);
}
public void ReceiveGetCachedCarteisanCommands(NetworkMessage message)
{
float[] positions = KinovaAPI.GetCachedCartesianCommands();
Debug.Log("<color=blue>Received:</color> got cached cart commands:"
+ positions[0] + ", "
+ positions[1] + ", "
+ positions[2] + ", "
+ positions[3] + ", "
+ positions[4] + ", "
+ positions[5] + ", "
+ positions[6] + ", "
+ positions[7]
);
}
// Create a server and listen on a port
public void SetupServer()
{
KinovaAPI.InitRobot();
NetworkServer.Listen(port);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_MOVE_ARM, ReceiveMoveArm);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_MOVE_ARM_NO_THETAY, ReceiveMoveArmNoThetaY);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_MOVE_ARM_HOME, ReceiveMoveArmHome);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_STOP_ARM, ReceiveStopArm);
if (!localRun)
{
videoChat.gameObject.SetActive(true);
videoChat.StartVideoChat();
}
isAtStartup = false;
Debug.Log("Server running listening on port " + port);
}
private void ReceiveCartesianPositionRequest(NetworkMessage netMsg)
{
var msg = netMsg.ReadMessage <RequestCartesianPositionMessage>(); // irrelevent, client doesn't need to send us anything except the request itself
RequestCartesianPositionMessage m = new RequestCartesianPositionMessage();
float[] positions = KinovaAPI.GetCartesianPositions();
m.x = positions[0];
m.y = positions[1];
m.z = positions[2];
m.thetaX = positions[3];
m.thetaY = positions[4];
m.thetaZ = positions[5];
m.fp1 = positions[6];
m.fp2 = positions[7];
m.fp3 = 0;
NetworkServer.SendToAll(MyMsgTypes.MSG_REQUEST_CARTESIAN_POSITION, m);
Debug.Log("Sending frozen position!");
}
// Create a server and listen on a port
public void SetupServer()
{
KinovaAPI.InitRobot();
NetworkServer.Listen(port);
//NetworkServer.RegisterHandler (MyMsgTypes.MSG_MOVE_ARM, ReceiveMoveArm);
//NetworkServer.RegisterHandler (MyMsgTypes.MSG_MOVE_ARM_NO_THETAY, ReceiveMoveArmNoThetaY);
//NetworkServer.RegisterHandler (MyMsgTypes.MSG_MOVE_ARM_HOME, ReceiveMoveArmHome);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_STOP_ARM, ReceiveStopArm);
// //Shawn testing 10.1.17
NetworkServer.RegisterHandler(MyMsgTypes.MSG_SET_ARM_POSITION, ReceiveSetArmPosition);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_SET_FINGER_POSITION, ReceiveSetFingerPosition);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_MOVE_ARM_UPDATE, ReceiveMoveArmUpdate);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_FREEZE_ARM_POSITION, ReceiveFreezeArmPosition);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_GET_CARTESIAN_POSITIONS, ReceiveGetCarteisanPositions);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_GET_CARTESIAN_COMMANDS, ReceiveGetCarteisanCommands);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_GET_CACHED_CARTESIAN_COMMANDS, ReceiveGetCachedCarteisanCommands);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_MOVE_ARM_CARTESIAN_POSITION_WITH_FINGERS, ReceiveMoveArmWithFingers);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_INIT_CLIENT_VIEW, ReceiveInitClientView);
NetworkServer.RegisterHandler((short)MyMsgTypes.MSG_CHAT, OnServerChatMessage);
NetworkServer.RegisterHandler(MyMsgTypes.MSG_REQUEST_CARTESIAN_POSITION, ReceiveCartesianPositionRequest);
//Testing
// NetworkServer.RegisterHandler (MyMsgTypes.MSG_MOVE_FINGERS, ReceiveMoveFingers);
videoChat.gameObject.SetActive(true);
videoChat.StartVideoChat();
isAtStartup = false;
Debug.Log("Server running listening on port " + port);
}
