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());
}
