// Update is called once per frame
void Update()
{
Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage();
Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1];
arr[0] = new Messages.geometry_msgs.TransformStamped();
tfmsg.transforms = arr;
Transform trans = trackedObject.transform;
emTransform ta = new emTransform(trans, ROS.GetTime(), frame_id, child_frame_id);
Messages.std_msgs.Header hdr = new Messages.std_msgs.Header();
hdr.frame_id = frame_id;
hdr.stamp = ROS.GetTime();
if (!using_gazebo)
{
hdr.stamp.data.sec += 18000;
}
tfmsg.transforms[0].header = hdr;
tfmsg.transforms[0].child_frame_id = child_frame_id;
tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform();
tfmsg.transforms[0].transform.translation = ta.origin.ToMsg();
//tfmsg.transforms[0].transform.translation.z += 1.0;
tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg();
tfmsg.Serialized = null;
tfPub.publish(tfmsg);
}
void Update()
{
if (publishtf)
{
_tfmsg.transforms[0].header.stamp = ROS.GetTime();
_tfmsg.transforms[0].header.stamp.data.sec += 18000; //windows time is dumb
//Debug.Log("Current time" + ROS.GetTime().data.sec);
tfPub.publish(_tfmsg);
}
if (hj == null)
{
hj = GameObject.Find(frame_id);
}
else if (!interactableItem.IsInteracting())
{
if (!startedInteracting)
{
//Debug.Log("Not interacting!");
Vector3 t = hj.transform.position;
Quaternion q = hj.transform.rotation;
transform.position = t;
transform.rotation = q;
}
if (startedInteracting)
{
Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage();
Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1];
arr[0] = new Messages.geometry_msgs.TransformStamped();
tfmsg.transforms = arr;
//Transform trans = trackedObj.transform;
emTransform ta = new emTransform(transform, ROS.GetTime(), "/world", "/look_at_frame");
Messages.std_msgs.Header hdr = new Messages.std_msgs.Header();
hdr.frame_id = "/world";
hdr.stamp = ROS.GetTime();
hdr.stamp.data.sec += 18000;
tfmsg.transforms[0].header = hdr;
tfmsg.transforms[0].child_frame_id = "/look_at_frame";
tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform();
tfmsg.transforms[0].transform.translation = ta.origin.ToMsg();
tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg();
tfmsg.Serialized = null;
tfPub.publish(tfmsg);
_tfmsg = tfmsg;
publishtf = true;
startedInteracting = false;
}
}
else
{
startedInteracting = true;
}
}
// Update is called once per frame
void Update()
{
pollCount++;
if (pollCount > FrameSkip)
{
pollCount = 0;
Poll();
lock (BroadcastList)
{
if (BroadcastList.Count != 0)
{
Messages.tf.tfMessage tfm = new Messages.tf.tfMessage();
tfm.transforms = BroadcastList.ToArray();
tf_pub.publish(tfm);
BroadcastList.Clear();
}
}
}
}
// Update is called once per frame
void Update()
{
//only handle one message per frame per update
Dictionary <string, gm.TransformStamped> tfs = new Dictionary <string, gm.TransformStamped>();
lock (transforms)
{
while (transforms.Count > 0)
{
Messages.tf.tfMessage tm = transforms.Dequeue();
foreach (gm.TransformStamped t in tm.transforms)
{
tfs[t.child_frame_id] = t;
}
}
}
emTransform[] tfz = Array.ConvertAll <gm.TransformStamped, emTransform>(tfs.Values.ToArray(), (a) => new emTransform(a));
foreach (emTransform tf in tfz)
{
if (!tf.frame_id.StartsWith("/"))
{
tf.frame_id = "/" + tf.frame_id;
}
if (!tf.child_frame_id.StartsWith("/"))
{
tf.child_frame_id = "/" + tf.child_frame_id;
}
if (IsVisible(tf.child_frame_id))
{
Vector3 pos = tf.UnityPosition.Value;
Quaternion rot = tf.UnityRotation.Value;
lock (tree)
{
if (!tree.ContainsKey(tf.child_frame_id))
{
Transform value1;
if (tree.TryGetValue(tf.frame_id, out value1))
{
Template.SetParent(value1);
}
else
{
Debug.LogWarning(string.Format("The parent ({0}) of {1} is not in the tree yet!", tf.frame_id, tf.child_frame_id));
}
Transform newframe = (Transform)Instantiate(Template, Template.localPosition, Template.localRotation);
hideChildrenInHierarchy(newframe);
#if UNITY_EDITOR
ObjectNames.SetNameSmart(newframe, tf.child_frame_id);
#endif
tree[tf.child_frame_id] = newframe;
tree[tf.child_frame_id].gameObject.GetComponentInChildren <TextMesh>(true).text = tf.child_frame_id;
}
Transform value;
if (tree.TryGetValue(tf.frame_id, out value))
{
value.gameObject.SetActive(true);
tree[tf.child_frame_id].SetParent(value, false);
}
if (value != null && !value.gameObject.activeInHierarchy)
{
value.gameObject.SetActive(true);
}
tree[tf.child_frame_id].gameObject.SetActive(true);
tree[tf.child_frame_id].localPosition = pos;
tree[tf.child_frame_id].localRotation = rot;
tree[tf.child_frame_id].GetChild(0).localScale = new Vector3(axis_scale, axis_scale, axis_scale);
}
}
}
AxesHider.update(show_axes);
LabelCorrector.update(show_labels);
TransformLineConnector.update(show_lines);
Root.GetChild(0).localScale = new Vector3(axis_scale, axis_scale, axis_scale);
}
