public void Newtf(TFMsg msg, string host)
{
TransformStampedMsg[] msgs = msg.Gettransforms();
foreach (TransformStampedMsg tf in msgs)
{
string nameParent = tf.Getheader().GetFrameId().Trim(_specialChar) + "_" + host;
string nameTf = tf.GetChild_frame_id().Trim(_specialChar) + "_" + host;
if (nameTf != null)
{
GameObject go = GameObject.Find(nameTf);
if (go == null)
{
CreateTf(nameTf, nameParent, tf.Gettransform().GetMatrix4x4());
}
else
{
if (go.transform.parent == null)
{
GameObject parent = GameObject.Find(nameParent);
if (parent == null)
{
go.transform.parent = CreateTf(nameParent, "", Matrix4x4.identity);
}
else
{
go.transform.parent = parent.transform;
}
}
go.transform.FromMatrix(tf.Gettransform().GetMatrix4x4());
}
}
}
}
IEnumerator SendTfs()
{
while (Application.isPlaying)
{
foreach (ROS ros in clients)
{
List <TransformStampedMsg> _transforms = new List <TransformStampedMsg>();
foreach (Transform tf in tfNodesRegistered)
{
if (tf != null)
{
string parent_name = "map";
if (tf.parent != null)
{
parent_name = tf.parent.name;
}
_transforms.Add(new TransformStampedMsg(new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0),
parent_name),
tf.name, new TransformMsg(tf)));
}
else
{
tfNodesRegistered.Remove(tf);
}
}
TFMsg msg = new TFMsg(_transforms.ToArray());
ros.Publish(Tf_pub.GetMessageTopic(), msg);
}
Log("Tfs updated.", LogLevel.Developer);
yield return(new WaitForSeconds(updateRate));
}
}
public void NewTf(TFMsg _msg, string _ip)
{
TransformStampedMsg[] msgs = _msg.Gettransforms();
foreach (TransformStampedMsg tf in msgs)
{
string nameParent = tf.Getheader().GetFrameId().Trim(specialChar);
string nameTf = tf.GetChild_frame_id().Trim(specialChar);
if (nameTf != null)
{
GameObject go = GameObject.Find(nameTf);
if (go == null)
{
CreateTf(nameTf, nameParent, tf.Gettransform());
}
else
{
if (go.transform.parent.name != nameParent)
{
GameObject parent = GameObject.Find(nameParent);
if (parent == null)
{
go.transform.parent = CreateTf(nameParent);
}
else
{
go.transform.parent = parent.transform;
}
}
go.transform.localPosition = tf.Gettransform().GetTranslation().GetVector3Unity();
go.transform.localRotation = tf.Gettransform().GetRotation().GetQuaternionUnity();
}
}
}
}
public static string ToYAMLString(TFMsg msg)
{
return(msg.ToYAMLString());
}