Пример #1
0
        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;
        }
Пример #2
0
        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;
        }