public void transformVector(string target_frame, Stamped <Vector3> stamped_in, ref Stamped <Vector3> stamped_out)
{
Transform trans = new Transform();
lookupTransform(target_frame, stamped_in.FrameId, stamped_in.Stamp, out trans);
Vector3 end = stamped_in.Data;
Vector3 origin = new Vector3(0, 0, 0);
Vector3 output = (trans * end) - (trans * origin);
if (stamped_out == null)
{
stamped_out = new Stamped <Vector3>();
}
stamped_out.Data = output;
stamped_out.Stamp = trans.Stamp;
stamped_out.FrameId = target_frame;
}
public void transformQuaternion(string target_frame, Stamped <Quaternion> stamped_in, ref Stamped <Quaternion> stamped_out)
{
Transform trans = new Transform();
lookupTransform(target_frame, stamped_in.FrameId, stamped_in.Stamp, out trans);
if (stamped_out == null)
{
stamped_out = new Stamped <Quaternion>();
}
stamped_out.Data = trans * stamped_in.Data;
stamped_out.Stamp = trans.Stamp;
stamped_out.FrameId = target_frame;
}