public void ReceiveTF(TFMessageMsg message) { foreach (var tf_message in message.transforms) { var frame_id = tf_message.header.frame_id + "/" + tf_message.child_frame_id; var tf = GetOrCreateTFStream(frame_id); tf.Add( tf_message.header.stamp.ToLongTime(), tf_message.transform.translation.From <FLU>(), tf_message.transform.rotation.From <FLU>() ); NotifyChanged(tf); } }
// Update is called once per frame void Update() { if (SystemStarter.Instance.calibrated) { //Vector3 relativePositionToParent = gameObject.transform.InverseTransformPoint(parentGameObject.transform.position); Vector3 relativePositionToParent = parentGameObject.transform.InverseTransformPoint(gameObject.transform.position); Quaternion relativeRotationToParent = Quaternion.Inverse(parentGameObject.transform.rotation) * gameObject.transform.rotation; tfMsg = new TFMessageMsg(new List <TransformStampedMsg>() { new TransformStampedMsg(new HeaderMsg(0, ROSTimeHelper.GetCurrentTime(), frame_id), child_frame_id, new TransformMsg(new Vector3Msg(relativePositionToParent.x, -relativePositionToParent.y, relativePositionToParent.z), new QuaternionMsg(-relativeRotationToParent.x, relativeRotationToParent.y, -relativeRotationToParent.z, relativeRotationToParent.w))) }); ROSCommunicationManager.Instance.ros.Publish(TFPublisher.GetMessageTopic(), tfMsg, debug_log: false); } }
public static string ToYAMLString(TFMessageMsg msg) { return(msg.ToYAMLString()); }