diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs index b6f17b9d..ccd01cec 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs @@ -1,14 +1,75 @@ +using System.Collections; +using System.Collections.Generic; using UnityEngine; using UnitySensors; -public class TFSensor : Sensor { +public class TFSensor : Sensor +{ + public struct TFData + { + public string frame_id_parent; + public string frame_id_child; + public Vector3 position; + public Quaternion rotation; + }; - [ReadOnly] - private Vector3 _position; + [SerializeField] + public string frame_id; + [SerializeField] + public TFSensor[] _children; - [ReadOnly] - private Quaternion _rotation; + private Transform _transform; - public Vector3 position { get => _position; } - public Quaternion rotation { get => _rotation; } + public TFData[] tf { get => GetTFData(); } + + protected override void Init() + { + _transform = transform; + base.Init(); + } + + protected override void UpdateSensor() + { + base.UpdateSensor(); + } + + public TFData[] GetTFData() + { + List tf = new List(); + + Matrix4x4 worldToLocalMatrix = _transform.worldToLocalMatrix; + Quaternion worldToLocalQuaternion = Quaternion.Inverse(_transform.rotation); + foreach (TFSensor child in _children) + { + tf.AddRange(child.GetTFData(frame_id, worldToLocalMatrix, worldToLocalQuaternion)); + } + return tf.ToArray(); + } + + public TFData[] GetTFData(string frame_id_parent, Matrix4x4 worldToLocalMatrix, Quaternion worldToLocalQuaternion) + { + List tf = new List(); + + TFData tfData; + tfData.frame_id_parent = frame_id_parent; + tfData.frame_id_child = frame_id; + tfData.position = worldToLocalMatrix * _transform.position; + tfData.rotation = worldToLocalQuaternion * _transform.rotation; + tf.Add(tfData); + + worldToLocalMatrix = _transform.worldToLocalMatrix; + worldToLocalQuaternion = Quaternion.Inverse(_transform.rotation); + foreach (TFSensor child in _children) + { + tf.AddRange(child.GetTFData(frame_id, worldToLocalMatrix, worldToLocalQuaternion)); + } + return tf.ToArray(); + } + + public void AddChild(TFSensor child) + { + List children = _children!=null ? new List(_children) : new List(); + children.Add(child); + _children = children.ToArray(); + } } \ No newline at end of file diff --git a/Assets/UnitySensors/Samples/TF/TF.unity b/Assets/UnitySensors/Samples/TF/TF.unity index d0c59dcf..3e7a405c 100644 --- a/Assets/UnitySensors/Samples/TF/TF.unity +++ b/Assets/UnitySensors/Samples/TF/TF.unity @@ -38,7 +38,7 @@ RenderSettings: m_ReflectionIntensity: 1 m_CustomReflection: {fileID: 0} m_Sun: {fileID: 0} - m_IndirectSpecularColor: {r: 0.44657826, g: 0.49641263, b: 0.57481676, a: 1} + m_IndirectSpecularColor: {r: 0.44657898, g: 0.4964133, b: 0.5748178, a: 1} m_UseRadianceAmbientProbe: 0 --- !u!157 &3 LightmapSettings: @@ -387,10 +387,6 @@ MonoBehaviour: _msg: transforms: [] _topicName: /tf - thisFrameID: base_link - rootTF: 1 - parentSensor: {fileID: 0} - parentFrameID: ground_truth --- !u!114 &304106768 MonoBehaviour: m_ObjectHideFlags: 0 @@ -404,6 +400,10 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: _frequency: 10 + frame_id: + _children: + - {fileID: 1983887115} + - {fileID: 1891329943} --- !u!114 &304106769 MonoBehaviour: m_ObjectHideFlags: 0 @@ -635,27 +635,8 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: _frequency: 10 ---- !u!114 &1891329944 -MonoBehaviour: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - m_GameObject: {fileID: 1891329939} - m_Enabled: 1 - m_EditorHideFlags: 0 - m_Script: {fileID: 11500000, guid: 0cde12085c739d548a159c84b40bbd2a, type: 3} - m_Name: - m_EditorClassIdentifier: - _frequency: 10 - _serializer: - _msg: - transforms: [] - _topicName: /tf - thisFrameID: velodyne_link - rootTF: 0 - parentSensor: {fileID: 304106762} - parentFrameID: base_link + frame_id: velodyne_link + _children: [] --- !u!1001 &1983887109 PrefabInstance: m_ObjectHideFlags: 0 @@ -731,27 +712,6 @@ Transform: type: 3} m_PrefabInstance: {fileID: 1983887109} m_PrefabAsset: {fileID: 0} ---- !u!114 &1983887114 -MonoBehaviour: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - m_GameObject: {fileID: 1822541744} - m_Enabled: 1 - m_EditorHideFlags: 0 - m_Script: {fileID: 11500000, guid: 0cde12085c739d548a159c84b40bbd2a, type: 3} - m_Name: - m_EditorClassIdentifier: - _frequency: 10 - _serializer: - _msg: - transforms: [] - _topicName: /tf - thisFrameID: camera - rootTF: 0 - parentSensor: {fileID: 304106762} - parentFrameID: base_link --- !u!114 &1983887115 MonoBehaviour: m_ObjectHideFlags: 0 @@ -765,3 +725,5 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: _frequency: 10 + frame_id: camera + _children: [] diff --git a/Assets/UnitySensorsROS/Editor/URDF2TFConverter.meta b/Assets/UnitySensorsROS/Editor/URDF2TFConverter.meta new file mode 100644 index 00000000..a6e51785 --- /dev/null +++ b/Assets/UnitySensorsROS/Editor/URDF2TFConverter.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 066c2801c2186214db0282dfe163b45d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs b/Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs new file mode 100644 index 00000000..e36309c7 --- /dev/null +++ b/Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs @@ -0,0 +1,136 @@ +using System.Xml; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using UnityEditor; + +using UnitySensors; +using UnitySensors.ROS; + +#if UNITY_EDITOR +class URDF2TFConverter : EditorWindow +{ + private enum Mode + { + FromTextAsset, + FromFilePath + } + + private Mode _mode; + private string _filePath; + private TextAsset _urdfFile; + + [MenuItem("UnitySensorsROS/Generate TF Objects...")] + public static void ShowWindow() + { + EditorWindow.GetWindow(typeof(URDF2TFConverter)); + } + + private void OnGUI() + { + GUILayout.Label("Setting", EditorStyles.boldLabel); + + EditorGUILayout.Space(); + + _mode = (Mode)EditorGUILayout.EnumPopup("Source", _mode); + + if (_mode == Mode.FromTextAsset) + { + _urdfFile = EditorGUILayout.ObjectField("URDF File", _urdfFile, typeof(TextAsset), true) as TextAsset; + } + else + { + _filePath = EditorGUILayout.TextField("URDF File Path", _filePath); + } + + EditorGUILayout.Space(); + + if (GUILayout.Button("Generate TF Objects")) + { + if (_mode == Mode.FromTextAsset && !_urdfFile) return; + Generate(); + } + } + + private void Generate() + { + XmlDocument doc = new XmlDocument(); + + if(_mode == Mode.FromTextAsset) + { + doc.LoadXml(_urdfFile.text); + } + else + { + doc.Load(_filePath); + } + + XmlNode robot_node = doc.SelectSingleNode("robot"); + GameObject robot_obj = new GameObject(); + Transform robot_trans = robot_obj.transform; + string robot_name = robot_node.Attributes.GetNamedItem("name").Value; + robot_obj.name = robot_name; + + Dictionary links = new Dictionary(); + Dictionary tfs = new Dictionary(); + links.Add(robot_name, robot_trans); + + XmlNodeList link_nodes = robot_node.SelectNodes("link"); + for (int i = 0; i < link_nodes.Count; i++) + { + GameObject link_obj = new GameObject(); + string link_name = link_nodes[i].Attributes.GetNamedItem("name").Value; + link_obj.name = link_name; + links.Add(link_name, link_obj.transform); + TFSensor tf = link_obj.AddComponent(); + tf.frame_id = link_name; + tfs.Add(link_name, tf); + if (i == 0) link_obj.AddComponent(); + } + + XmlNodeList joint_nodes = robot_node.SelectNodes("joint"); + for (int i = 0; i < joint_nodes.Count; i++) + { + string parent_name = joint_nodes[i].SelectSingleNode("parent").Attributes.GetNamedItem("link").Value; + string child_name = joint_nodes[i].SelectSingleNode("child").Attributes.GetNamedItem("link").Value; + links[child_name].parent = links[parent_name]; + tfs[parent_name].AddChild(tfs[child_name]); + + XmlNode origin_node = joint_nodes[i].SelectSingleNode("origin"); + if (origin_node != null) + { + XmlNode xyz_node = origin_node.Attributes.GetNamedItem("xyz"); + if (xyz_node != null) + { + string[] pos_str = xyz_node.Value.Split(' '); + Vector3 pos = new Vector3(-float.Parse(pos_str[1]), float.Parse(pos_str[2]), float.Parse(pos_str[0])); + links[child_name].localPosition = pos; + } + else + { + links[child_name].localPosition = Vector3.zero; + } + + XmlNode rpy_node = origin_node.Attributes.GetNamedItem("rpy"); + if (rpy_node != null) + { + string[] rot_str = rpy_node.Value.Split(' '); + Vector3 rot = new Vector3(-float.Parse(rot_str[1]), float.Parse(rot_str[2]), float.Parse(rot_str[0])); + links[child_name].localEulerAngles = rot * Mathf.Rad2Deg; + } + else + { + links[child_name].localEulerAngles = Vector3.zero; + } + } + } + + foreach (Transform link in links.Values) + { + if (link.parent) continue; + link.parent = robot_trans; + } + } +} + +#endif diff --git a/Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs.meta b/Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs.meta new file mode 100644 index 00000000..3e546a8e --- /dev/null +++ b/Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d126621331cb8aa408efd50703cf2926 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef b/Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef new file mode 100644 index 00000000..7f0a0d56 --- /dev/null +++ b/Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef @@ -0,0 +1,17 @@ +{ + "name": "UnitySensorsROSEditor", + "rootNamespace": "", + "references": [ + "GUID:e9a473e6ad03eae4a89800bf81bd1594", + "GUID:fa38f881a10f6314fa784b8975c16eff" + ], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [], + "noEngineReferences": false +} \ No newline at end of file diff --git a/Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef.meta b/Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef.meta new file mode 100644 index 00000000..af9d4b2e --- /dev/null +++ b/Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: f9b737200d878b748a40a8ec4b055255 +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs index 9faf8174..27e70079 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/TF/TFPublisher.cs @@ -3,42 +3,20 @@ namespace UnitySensors.ROS { - + [RequireComponent(typeof(TFSensor))] public class TFPublisher : Publisher { public string _topicName = "/tf"; - public string thisFrameID; - public bool rootTF; - public GameObject parentSensor; - public string parentFrameID; - - - private Vector3 position_rel; // local position relative to parentSensor - private Quaternion rotation_rel; // local rotation relative to parentSensor protected override void Init() { _ros.RegisterPublisher(_topicName); - if (rootTF) - { - _serializer.Init(parentFrameID); - } - else - { - _serializer.Init(parentFrameID); - position_rel = parentSensor.transform.InverseTransformPoint(transform.position); - rotation_rel = Quaternion.Inverse(parentSensor.transform.rotation) * transform.rotation; - } + _serializer.Init(); } protected override void Publish(float time) { - if (rootTF) - { - position_rel = transform.position; - rotation_rel = transform.rotation; - } - _serializer.Serialize(time, thisFrameID, position_rel, rotation_rel); + _serializer.Serialize(time, _sensor.tf); _ros.Publish(_topicName, _serializer.msg); } } diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs index e0cded7a..470cb197 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/TF/TFSerializer.cs @@ -1,8 +1,12 @@ +using System.Collections; +using System.Collections.Generic; using RosMessageTypes.Geometry; using RosMessageTypes.Tf2; using Unity.Robotics.ROSTCPConnector.ROSGeometry; using UnityEngine; +using TFData = TFSensor.TFData; + namespace UnitySensors.ROS { @@ -17,24 +21,29 @@ public class TFSerializer : Serializer public TFMessageMsg msg { get => _msg; } - public void Init(string parent_frame_id) + public void Init() { _msg = new TFMessageMsg(); - _msg.transforms = new TransformStampedMsg[1]; _header = new AutoHeader(); - _header.Init(parent_frame_id); + _header.Init(""); } - public TFMessageMsg Serialize(float time, string thisFrameID, Vector3 position, Quaternion rotation) + public TFMessageMsg Serialize(float time, TFData[] tf) { _header.Serialize(time); - TransformStampedMsg tfStamped = new TransformStampedMsg(); - tfStamped.header = _header.header; - tfStamped.child_frame_id = thisFrameID; - tfStamped.transform.translation = position.To(); - tfStamped.transform.rotation = rotation.To(); - _msg.transforms[0] = tfStamped; + List transforms = new List(); + 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(); + transform.transform.rotation = tfData.rotation.To(); + transforms.Add(transform); + } + _msg.transforms = transforms.ToArray(); return _msg; } }