Esempio 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);
    }
Esempio 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;
        }
    }
 /// <summary>
 /// add TF data, they will publish next Update().
 /// </summary>
 /// <param name="tfdata"></param>
 public void AddTf(GM.TransformStamped tfdata)
 {
     lock (BroadcastList)
     {
         tfdata.header.seq = tfSeq;
         BroadcastList.Add(tfdata);
         tfSeq++;
     }
 }
 /// <summary>
 /// get blank TransformStamped all member is filled (no null pointer)
 /// you can just fill members such as x,y,z
 /// </summary>
 /// <returns>Blank TransformStamped data for addTf</returns>
 public GM.TransformStamped BlankTf()
 {
     GM.TransformStamped tfdata = new GM.TransformStamped();
     tfdata.header                = new Messages.std_msgs.Header();
     tfdata.transform             = new GM.Transform();
     tfdata.header.stamp          = ROS.GetTime();
     tfdata.transform             = new GM.Transform();
     tfdata.transform.translation = new GM.Vector3();
     tfdata.transform.rotation    = new GM.Quaternion();
     return(tfdata);
 }
    /// <summary>
    /// Poll coordinate data and push to TF data (User Logic is here)
    /// </summary>
    void Poll()
    {
        GM.TransformStamped tfdata = BlankTf();
        tfdata.header.frame_id         = "tango_base";
        tfdata.child_frame_id          = "tango_device";
        tfdata.transform.translation.x = TangoCamera.position.x;
        tfdata.transform.translation.y = TangoCamera.position.y;
        tfdata.transform.translation.z = TangoCamera.position.z;
        tfdata.transform.rotation.x    = TangoCamera.rotation.x;
        tfdata.transform.rotation.y    = TangoCamera.rotation.y;
        tfdata.transform.rotation.z    = TangoCamera.rotation.z;
        tfdata.transform.rotation.w    = TangoCamera.rotation.w;
        AddTf(tfdata);

        //FYI
        //to deepcopy tf data,use fllowing.
        //tfdata_to.Deserialize(tfdata_from.Serialize());
    }
 public emTransform(gm.TransformStamped msg) : this(new emQuaternion(msg.transform.rotation), new emVector3(msg.transform.translation), msg.header.Stamp, msg.header.Frame_id, msg.child_frame_id)
 {
 }