void Awake()
{
toUnity = MatrixExtension.RosToUnity();
// toRos = transform.localToWorldMatrix.ToUnity ();
toRos = toUnity.inverse;
// Debug.Log ( "ToUnity: " + toUnity );
// Debug.Log ( "ToRos: " + toRos );
// Debug.Log ( transform.worldToLocalMatrix.ToUnity () );
// Debug.Log ( transform.worldToLocalMatrix.ToUnity ().inverse );
// Debug.Log ( "forward to ros: " + Vector3.forward.ToRos () );
// Quaternion q = QuaternionToRos ( transform.rotation );
// Vector3 v = q * Vector3.forward;
// Debug.Log ( "v " + v );
// Debug.Log ( "rot " + transform.rotation + " q " + q );
// Debug.Log ( "forward " + Vector3.up.ToRos () );
}