// 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; } }
/// <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) { }