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; }