public static string ToYAMLString(Ctrl_InputMsg msg)
{
return(msg.ToYAMLString());
}
/* Sets up a TCP socket connection with the server running on the machine runing
* the control algorithm.
* */
// public IEnumerator SetupSocket()
// {
// Debug.Log ("Setting the socket up");
// yield return null;
// try{
// #if notSelf
// mySocket = new TcpClient (IPAddress.Parse(IPaddress).ToString(), Port);
// theStream = mySocket.GetStream();
//
// theWriter = new StreamWriter(theStream);
//
// #endif
// Ready = true;
// }
// catch (Exception e) {
// Debug.Log("Socket error: " + e);
// }
// }
/* Send sensor data as feedback to the control algorithm. The data sent includes the orientation of the vehicle,
* acceleration in all three dimensions, depth under water, and forward velocity in local frame.
* */
void SendData()
{
try {
Vector3 CurRot = transform.parent.transform.rotation.eulerAngles;
// Debug.Log("The angles are: " + CurRot);
if (CurRot.x > 180.0f)
{
CurRot.x -= 360.0f;
}
if (CurRot.y > 180.0f)
{
CurRot.y -= 360.0f;
}
if (CurRot.z > 180.0f)
{
CurRot.z -= 360.0f;
}
CurRot *= (float)Math.PI / 180.0f;
Vector3 curVelocity = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().velocity);
Vector3 CurAcc = (curVelocity - prevVelocity) / Time.deltaTime;
prevVelocity = curVelocity;
// Vector3 Omega = (CurRot - prevRot)/Time.deltaTime;
Vector3 Omega = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().angularVelocity);
prevRot = CurRot;
// string temp = (-CurRot.x).ToString("+000.00;-000.00") + " " + (-CurRot.z).ToString("+000.00;-000.00") + " "
// + (CurRot.y).ToString("+000.00;-000.00") + " " + CurAcc.x.ToString("+000.00;-000.00") + " "
// + CurAcc.z.ToString("+000.00;-000.00") + " " + (-CurAcc.y).ToString("+000.00;-000.00") + " "
// + (-transform.parent.position.y).ToString("+000.00;-000.00") + " "
// + transform.parent.GetComponent<Rigidbody>().velocity.x.ToString("+000.00;-000.00") + " $";
// Debug.Log(temp);
float modifiedDepth = (-transform.parent.position.y * 15.0f) + 930.0f;
//Uncomment for old controller
// float[] angular = new float[]{-CurRot.x, CurRot.z, CurRot.y};
// float[] linear = new float[]{CurAcc.x, -CurAcc.z, modifiedDepth};
//
// msg = new CombinedMsg(angular, linear);
//For new controller
float[] velocity = new float[] { curVelocity.x, -curVelocity.z, -curVelocity.y };
float[] acceleration = new float[] { CurAcc.x, -CurAcc.z, -CurAcc.y };
float[] angle = new float[] { -CurRot.x, CurRot.z, CurRot.y };
float[] omega = new float[] { -Omega.x, Omega.z, Omega.y };
msg = new Ctrl_InputMsg(velocity, acceleration, angle, omega, modifiedDepth);
#if notSelf
Debug.Log("Sending: Depth = " + modifiedDepth);
Debug.Log("Sending: Force = " + CurAcc.y * 15000);
Debug.Log("Sending to topic: " + ROSPublisher.GetMessageTopic());
obj.GetComponent <ROS_Initialize> ().ros.Publish(ROSPublisher.GetMessageTopic(), msg);
#endif
}
catch (Exception e) {
Debug.Log("Socket error: " + e);
}
}
