-
Notifications
You must be signed in to change notification settings - Fork 39
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #47 from Field-Robotics-Japan/feature/urdf2tf
Test PR 2
- Loading branch information
Showing
9 changed files
with
278 additions
and
89 deletions.
There are no files selected for viewing
75 changes: 68 additions & 7 deletions
75
Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<TFData> tf = new List<TFData>(); | ||
|
|
||
| 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<TFData> tf = new List<TFData>(); | ||
|
|
||
| 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<TFSensor> children = _children!=null ? new List<TFSensor>(_children) : new List<TFSensor>(); | ||
| children.Add(child); | ||
| _children = children.ToArray(); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
136 changes: 136 additions & 0 deletions
136
Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<string, Transform> links = new Dictionary<string, Transform>(); | ||
| Dictionary<string, TFSensor> tfs = new Dictionary<string, TFSensor>(); | ||
| 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<TFSensor>(); | ||
| tf.frame_id = link_name; | ||
| tfs.Add(link_name, tf); | ||
| if (i == 0) link_obj.AddComponent<TFPublisher>(); | ||
| } | ||
|
|
||
| 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 |
11 changes: 11 additions & 0 deletions
11
Assets/UnitySensorsROS/Editor/URDF2TFConverter/URDF2TFConverter.cs.meta
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
17 changes: 17 additions & 0 deletions
17
Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
| } |
7 changes: 7 additions & 0 deletions
7
Assets/UnitySensorsROS/Editor/UnitySensorsROSEditor.asmdef.meta
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
Oops, something went wrong.