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