public QuaternionMsg(Quaternion q, bool fromUnity = false)
{
if (fromUnity)
{
q = TfUtils.TransormQuaternionUnity2ROS(q);
}
_x = q.x;
_y = q.y;
_z = q.z;
_w = q.w;
}
public Quaternion GetQuaternionUnity()
{
return(TfUtils.TransormQuaternionROS2Unity(new Quaternion(_x, _y, _z, _w)));
}
public Vector3 GetRotationEulerUnity(int solution)
{
Vector3 rotationEuler = TfUtils.GetEulerYPR(new Quaternion(_x, _y, _z, _w), solution) * Mathf.Rad2Deg;
return(new Vector3(rotationEuler.z, -rotationEuler.x, -rotationEuler.y));
}
public Vector3 GetRotationEulerROS(int solution)
{
return(TfUtils.GetEulerYPR(new Quaternion(_x, _y, _z, _w), solution));
}