Merge pull request #132 from Autumn60/hotfix/tf_bug

Hotfix/tf bug

Author: Autumn60

Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TF.cs

@@ -54,7 +54,7 @@ public TFData[] GetTFData(string frame_id_parent, Matrix4x4 worldToLocalMatrix,
             TFData tfData_self;
             tfData_self.frame_id_parent = frame_id_parent;
             tfData_self.frame_id_child = _frame_id;
-            tfData_self.position = worldToLocalMatrix * _transform.position;
+            tfData_self.position = (Vector3)(worldToLocalMatrix * new Vector4(_transform.position.x, _transform.position.y, _transform.position.z, 1.0f));
             tfData_self.rotation = worldToLocalQuaternion * _transform.rotation;
             tfData.Add(tfData_self);
 

Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFMsgSerializer.cs

@@ -46,43 +46,4 @@ public override TFMessageMsg Serialize()
             return _msg;
         }
     }
-    /*
-    [System.Serializable]
-    public class TFSerializer : Serializer
-    {
-
-        [SerializeField]
-        private TFMessageMsg _msg;
-
-        private AutoHeader _header;
-
-        public TFMessageMsg msg { get => _msg; }
-
-        public void Init()
-        {
-            _msg = new TFMessageMsg();
-            _header = new AutoHeader();
-
-            _header.Init("");
-        }
-
-        public TFMessageMsg Serialize(float time, TFData[] tf)
-        {
-            _header.Serialize(time);
-            List<TransformStampedMsg> transforms = new List<TransformStampedMsg>();
-            foreach(TFData tfData in tf)
-            {
-                TransformStampedMsg transform = new TransformStampedMsg();
-                transform.header = _header.header;
-                transform.header.frame_id = tfData.frame_id_parent;
-                transform.child_frame_id = tfData.frame_id_child;
-                transform.transform.translation = tfData.position.To<FLU>();
-                transform.transform.rotation = tfData.rotation.To<FLU>();
-                transforms.Add(transform);
-            }
-            _msg.transforms = transforms.ToArray();
-            return _msg;
-        }
-    }
-    */
 }