Merge pull request #89 from Autumn60/test/restruct

Add TF scripts

Assets/UnitySensors/Editor/TF/TFEditor.cs

@@ -0,0 +1,15 @@
+using UnityEditor;
+using UnitySensors.Sensor.TF;
+
+namespace UnitySensors.Editor
+{
+    [CustomEditor(typeof(TF))]
+    public class TFEditor : UnityEditor.Editor
+    {
+        public override void OnInspectorGUI()
+        {
+            base.OnInspectorGUI();
+            EditorGUILayout.HelpBox("TF does not use \"Frequency\" param.", MessageType.Info);
+        }
+    }
+}

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

@@ -1,7 +1,79 @@
 using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
-using UnitySensors;
+using UnitySensors.Attribute;
+
+namespace UnitySensors.Sensor.TF
+{
+    [System.Serializable]
+    public struct TFData
+    {
+        public string frame_id_parent;
+        public string frame_id_child;
+        public Vector3 position;
+        public Quaternion rotation;
+    };
+
+    public class TF : UnitySensor
+    {
+        [SerializeField]
+        private string _frame_id;
+        [SerializeField]
+        private TF[] _children;
+
+        private Transform _transform;
+
+        protected override void Init()
+        {
+            _transform = this.transform;
+        }
+
+        protected override void UpdateSensor()
+        {
+        }
+
+        public TFData[] GetTFData()
+        {
+            List<TFData> tfData = new List<TFData>();
+
+            Matrix4x4 worldToLocalMatrix = _transform.worldToLocalMatrix;
+            Quaternion worldToLocalQuaternion = Quaternion.Inverse(_transform.rotation);
+
+            foreach (TF child in _children)
+            {
+                tfData.AddRange(child.GetTFData(_frame_id, worldToLocalMatrix, worldToLocalQuaternion));
+            }
+
+            return tfData.ToArray();
+        }
+
+        public TFData[] GetTFData(string frame_id_parent, Matrix4x4 worldToLocalMatrix, Quaternion worldToLocalQuaternion)
+        {
+            List<TFData> tfData = new List<TFData>();
+
+            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.rotation = worldToLocalQuaternion * _transform.rotation;
+            tfData.Add(tfData_self);
+
+            worldToLocalMatrix = _transform.worldToLocalMatrix;
+            worldToLocalQuaternion = Quaternion.Inverse(_transform.rotation);
+
+            foreach (TF child in _children)
+            {
+                tfData.AddRange(child.GetTFData(_frame_id, worldToLocalMatrix, worldToLocalQuaternion));
+            }
+
+            return tfData.ToArray();
+        }
+
+        protected override void OnSensorDestroy()
+        {
+        }
+    }
+}
 
 /*
 public class TFSensor : Sensor

Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFMsgPublisher.cs

@@ -0,0 +1,16 @@
+using UnityEngine;
+
+using RosMessageTypes.Tf2;
+
+using UnitySensors.Sensor.TF;
+using UnitySensors.ROS.Serializer.TF;
+
+namespace UnitySensors.ROS.Publisher.TF
+{
+    using TFSensor = Sensor.TF.TF;
+
+    [RequireComponent(typeof(TFSensor))]
+    public class TFMsgPublisher : RosMsgPublisher<TFSensor, TFMsgSerializer, TFMessageMsg>
+    {
+    }
+}

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

@@ -1,14 +1,50 @@
-using System.Collections;
 using System.Collections.Generic;
+
+using UnityEngine;
+
+using Unity.Robotics.ROSTCPConnector.ROSGeometry;
+using RosMessageTypes.Std;
 using RosMessageTypes.Geometry;
 using RosMessageTypes.Tf2;
-using Unity.Robotics.ROSTCPConnector.ROSGeometry;
-using UnityEngine;
 
-//using TFData = TFSensor.TFData;
+using UnitySensors.Sensor.TF;
 
-namespace UnitySensors.ROS
+namespace UnitySensors.ROS.Serializer.TF
 {
+    using TFSensor = Sensor.TF.TF;
+
+    [System.Serializable]
+    public class TFMsgSerializer : RosMsgSerializer<TFSensor, TFMessageMsg>
+    {
+        [SerializeField]
+        private HeaderSerializer _header;
+
+        public override void Init(TFSensor sensor)
+        {
+            base.Init(sensor);
+            _header.Init(sensor);
+        }
+
+        public override TFMessageMsg Serialize()
+        {
+            HeaderMsg headerMsg = _header.Serialize();
+            List<TransformStampedMsg> transforms = new List<TransformStampedMsg>();
+
+            TFData[] tfData = sensor.GetTFData();
+            foreach(TFData data in tfData)
+            {
+                TransformStampedMsg transform = new TransformStampedMsg();
+                transform.header = headerMsg;
+                transform.header.frame_id = data.frame_id_parent;
+                transform.child_frame_id = data.frame_id_child;
+                transform.transform.translation = data.position.To<FLU>();
+                transform.transform.rotation = data.rotation.To<FLU>();
+                transforms.Add(transform);
+            }
+            _msg.transforms = transforms.ToArray();
+            return _msg;
+        }
+    }
     /*
     [System.Serializable]
     public class TFSerializer : Serializer