Merge pull request #133 from Field-Robotics-Japan/hotfix/tfMsgSerizlier

Hotfix/tf msg serizlier
Field-Robotics-Japan · Feb 14, 2024 · 416e40f · 416e40f
2 parents 44bb909 + 47c4a96
commit 416e40f
Show file tree

Hide file tree

Showing 2 changed files with 1 addition and 40 deletions.
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TF.cs b/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);
 

diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFMsgSerializer.cs b/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;
-        }
-    }
-    */
 }