Exemplo n.º 1
0
    // 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);
    }
Exemplo n.º 2
0
    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;
        }
    }
Exemplo n.º 3
0
    // 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();
                }
            }
        }
    }
Exemplo n.º 4
0
    // 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);
    }