/* 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;
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 = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().angularVelocity);
prevRot = CurRot;
float modifiedDepth = (-transform.parent.position.y * 15.0f);//+930.0f;
#region for old controller
//Uncomment for old controller
float[] angular = new float[] { -CurRot.x, CurRot.z, CurRot.y };
float[] linear = new float[] { CurAcc.x, -CurAcc.z, -CurAcc.y };
float depth = modifiedDepth;
msg = new CombinedMsg(angular, linear, depth);
//Debug.Log("combined message : "+msg);
#endregion
#region for new controller
//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);
#endregion
#if notSelf
obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(ROSPublisher.GetMessageTopic(), msg);
#endif
}
catch (Exception e)
{
Debug.Log("Socket error: " + e);
}
}
public static string ToYAMLString(CombinedMsg msg)
{
return(msg.ToYAMLString());
}