diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs index 8f0d6028..313357d8 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/IMU/IMUSensor.cs @@ -30,13 +30,19 @@ public class IMUSensor : Sensor public Vector3 angularVelocity { get => _angularVelocity; } public Vector3 localVelocity { get => _transform.InverseTransformDirection(_velocity); } - public Vector3 localAcceleration { get => _transform.InverseTransformDirection(_acceleration); } + public Vector3 localAcceleration { get => _transform.InverseTransformDirection(_acceleration.normalized) * _acceleration.magnitude; } private float _dt { get => base._frequency_inv; } + private Vector3 _gravity; + private float _gravityMagnitude; + protected override void Init() { _transform = this.transform; + + _gravity = Physics.gravity; + _gravityMagnitude = _gravity.magnitude; } protected override void UpdateSensor() @@ -46,7 +52,7 @@ protected override void UpdateSensor() _velocity = (_position - _position_last) / _dt; _acceleration = (_velocity - _velocity_last) / _dt; - _acceleration += _transform.InverseTransformVector(Physics.gravity); + _acceleration += _transform.InverseTransformDirection(_gravity).normalized * _gravityMagnitude; Quaternion rotation_delta = Quaternion.Inverse(_rotation_last) * _rotation; rotation_delta.ToAngleAxis(out float angle, out Vector3 axis); diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs index 59cd2f5f..f57fe892 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs @@ -55,6 +55,9 @@ public class LivoxSensor : Sensor private uint _randomSeed; private int _pointsNum; public uint pointsNum { get=>(uint)(_pointsNum);} + public float minDistance { get => _minDistance; } + public float maxDistance { get => _maxDistance; } + public float maxIntensity { get => _maxIntensity; } protected override void Init() { diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs index 4bfbe674..e2a7d719 100644 --- a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs +++ b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs @@ -43,9 +43,13 @@ public class VelodyneSensor : Sensor private uint _randomSeed; private int _pointsNum; + public uint pointsNum { get => (uint)_pointsNum; } public int layersNum { get => _scanPattern.numOfLayer; } public int azimuthResolution { get => _scanPattern.azimuthResolution; } + public float minDistance { get => _minDistance; } + public float maxDistance { get => _maxDistance; } + public float maxIntensity { get => _maxIntensity; } protected override void Init() { 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/IMU/IMU.unity b/Assets/UnitySensors/Samples/IMU/IMU.unity index 8cbffa30..3d8e5d61 100644 --- a/Assets/UnitySensors/Samples/IMU/IMU.unity +++ b/Assets/UnitySensors/Samples/IMU/IMU.unity @@ -313,6 +313,16 @@ PrefabInstance: propertyPath: m_RootOrder value: 2 objectReference: {fileID: 0} + - target: {fileID: 8550806465241961968, guid: 884f57c81164231419af9316aaca015d, + type: 3} + propertyPath: m_LocalScale.y + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 8550806465241961968, guid: 884f57c81164231419af9316aaca015d, + type: 3} + propertyPath: m_LocalScale.z + value: 1 + objectReference: {fileID: 0} - target: {fileID: 8550806465241961968, guid: 884f57c81164231419af9316aaca015d, type: 3} propertyPath: m_LocalPosition.x 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/GPS/GPSPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NMEAPublisher.cs similarity index 91% rename from Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs rename to Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NMEAPublisher.cs index 911da026..201c494c 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NMEAPublisher.cs @@ -9,7 +9,7 @@ namespace UnitySensors.ROS { [RequireComponent(typeof(GPSSensor))] [ExecuteAlways] - public class GPSPublisher : Publisher + public class NMEAPublisher : Publisher { [SerializeField] private string _topicName = "gnss/raw_data"; @@ -17,7 +17,7 @@ public class GPSPublisher : Publisher private string _frame_id = "gnss_link"; [SerializeField] - private GPSSerializer _serializer_gps; + private NMEASerializer _serializer_gps; protected override void Start() { diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NMEAPublisher.cs.meta similarity index 100% rename from Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs.meta rename to Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NMEAPublisher.cs.meta diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NavSatFixPublisher.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NavSatFixPublisher.cs new file mode 100644 index 00000000..739088fd --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NavSatFixPublisher.cs @@ -0,0 +1,33 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +using RosMessageTypes.Sensor; + +namespace UnitySensors.ROS +{ + [RequireComponent(typeof(GPSSensor))] + public class NavSatFixPublisher : Publisher + { + [SerializeField] + private string _topicName = "gnss/raw_data"; + [SerializeField] + private string _frame_id = "gnss_link"; + + [SerializeField] + private NavSatFixSerializer _serializer_navsat; + + protected override void Init() + { + _ros.RegisterPublisher(_topicName); + _serializer_navsat.Init(_frame_id); + } + + protected override void Publish(float time) + { + _serializer_navsat.Serialize(time, _sensor.coordinate); + _ros.Publish(_topicName, _serializer_navsat.msg); + } + } +} + diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NavSatFixPublisher.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NavSatFixPublisher.cs.meta new file mode 100644 index 00000000..c681402e --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NavSatFixPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 397c445553a6f3a40ac4d6892894ff7f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + 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/GPS/GPSSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NMEASerializer.cs similarity index 96% rename from Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/GPSSerializer.cs rename to Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NMEASerializer.cs index ffe56e3b..aba9a71d 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/GPSSerializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NMEASerializer.cs @@ -10,7 +10,7 @@ namespace UnitySensors.ROS { [System.Serializable] - public class GPSSerializer : Serializer + public class NMEASerializer : Serializer { [SerializeField] private NMEAFormatManager _format; diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/GPSSerializer.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NMEASerializer.cs.meta similarity index 100% rename from Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/GPSSerializer.cs.meta rename to Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NMEASerializer.cs.meta diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NavSatFixSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NavSatFixSerializer.cs new file mode 100644 index 00000000..ba0e9760 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NavSatFixSerializer.cs @@ -0,0 +1,63 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +using RosMessageTypes.Sensor; + +namespace UnitySensors.ROS +{ + [System.Serializable] + public class NavSatFixSerializer : Serializer + { + enum Status + { + NO_FIX, + FIX, + SBAS_FIX, + GBAS_FIX + } + + enum Service + { + GPS, + GLONASS, + COMPASS, + GALILEO + } + + [SerializeField] + private Status _status; + [SerializeField] + private Service _service; + + private NavSatFixMsg _msg; + private AutoHeader _header; + + private uint _service_num; + + public NavSatFixMsg msg { get => _msg; } + + public void Init(string frame_id) + { + _msg = new NavSatFixMsg(); + _header = new AutoHeader(); + + _msg.status = new NavSatStatusMsg(); + _msg.status.service = (ushort) Mathf.Pow(2, (int)(_service)); + _msg.position_covariance = new double[9]; + + _header.Init(frame_id); + } + + public void Serialize(float time, GeoCoordinate coordinate) + { + _header.Serialize(time); + _msg.header = _header.header; + _msg.status.status = (sbyte) ((int)(_status) - 1); + _msg.latitude = coordinate.latitude; + _msg.longitude = coordinate.longitude; + _msg.altitude = coordinate.altitude; + _msg.position_covariance_type = 0; + } + } +} \ No newline at end of file diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NavSatFixSerializer.cs.meta b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NavSatFixSerializer.cs.meta new file mode 100644 index 00000000..9e616108 --- /dev/null +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/GPS/NavSatFixSerializer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 02af565af39100d4895be7927f55c12f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs index bba6f0d9..7c1a6607 100644 --- a/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs +++ b/Assets/UnitySensorsROS/Runtime/Scripts/Serializers/LiDAR/Velodyne/VelodyneMsgSerializer.cs @@ -64,7 +64,11 @@ public VelodyneScanMsg Serialize(float time) _msg.header = _header.header; - uint sec = _header.header.stamp.sec; +#if ROS2 + int sec = (int)_header.header.stamp.sec; +#else + uint sec = (uint)_header.header.stamp.sec; +# endif uint nanosec = _header.header.stamp.nanosec; for (int i = 0; i < _msg.packets.Length; i++) 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; } } diff --git a/Assets/UnitySensorsROS/Samples/GPS/GPS_ros.unity b/Assets/UnitySensorsROS/Samples/GPS/GPS_ros.unity index 232ac085..ae3051a2 100644 --- a/Assets/UnitySensorsROS/Samples/GPS/GPS_ros.unity +++ b/Assets/UnitySensorsROS/Samples/GPS/GPS_ros.unity @@ -217,23 +217,6 @@ Transform: m_Father: {fileID: 0} m_RootOrder: 1 m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0} ---- !u!114 &873899251 -MonoBehaviour: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - m_GameObject: {fileID: 0} - m_Enabled: 1 - m_EditorHideFlags: 0 - m_Script: {fileID: 11500000, guid: 3a43f2130f8dec04da1eb2fbd26724c9, type: 3} - m_Name: GPGGA(Clone) - m_EditorClassIdentifier: - _timeMode: 0 - _quality: 0 - _satelliteNum: 8 - _HDOP: 0 - _geoidHeight: 36.7071 --- !u!1 &1080544182 GameObject: m_ObjectHideFlags: 0 @@ -299,6 +282,7 @@ GameObject: - component: {fileID: 1243027675} - component: {fileID: 1243027674} - component: {fileID: 1243027673} + - component: {fileID: 1243027676} m_Layer: 0 m_Name: GPS m_TagString: Untagged @@ -313,7 +297,7 @@ MonoBehaviour: m_PrefabInstance: {fileID: 0} m_PrefabAsset: {fileID: 0} m_GameObject: {fileID: 1243027672} - m_Enabled: 1 + m_Enabled: 0 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 0cd1737f876f48b4989fb62329fbe050, type: 3} m_Name: @@ -323,10 +307,9 @@ MonoBehaviour: _frame_id: gnss_link _serializer_gps: _format: - scriptableObject: {fileID: 11400000, guid: 397f872695a0b93498193d95c6ff737c, - type: 2} - _parametersCache: '{"_timeMode":0,"_quality":0,"_satelliteNum":8,"_HDOP":0.0,"_geoidHeight":36.70709991455078}' - _scriptableObject: {fileID: 873899251} + scriptableObject: {fileID: 0} + _parametersCache: + _scriptableObject: {fileID: 0} --- !u!114 &1243027674 MonoBehaviour: m_ObjectHideFlags: 0 @@ -365,6 +348,24 @@ Transform: m_Father: {fileID: 0} m_RootOrder: 2 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &1243027676 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1243027672} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 397c445553a6f3a40ac4d6892894ff7f, type: 3} + m_Name: + m_EditorClassIdentifier: + _frequency: 10 + _topicName: gnss/raw_data + _frame_id: gnss_link + _serializer_navsat: + _status: 0 + _service: 3 --- !u!1 &1274738610 GameObject: m_ObjectHideFlags: 0 diff --git a/Assets/UnitySensors/Samples/TF.meta b/Assets/UnitySensorsROS/Samples/TF.meta similarity index 100% rename from Assets/UnitySensors/Samples/TF.meta rename to Assets/UnitySensorsROS/Samples/TF.meta diff --git a/Assets/UnitySensors/Samples/TF/TF.unity b/Assets/UnitySensorsROS/Samples/TF/TF.unity similarity index 94% rename from Assets/UnitySensors/Samples/TF/TF.unity rename to Assets/UnitySensorsROS/Samples/TF/TF.unity index d0c59dcf..3e7a405c 100644 --- a/Assets/UnitySensors/Samples/TF/TF.unity +++ b/Assets/UnitySensorsROS/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/UnitySensors/Samples/TF/TF.unity.meta b/Assets/UnitySensorsROS/Samples/TF/TF.unity.meta similarity index 100% rename from Assets/UnitySensors/Samples/TF/TF.unity.meta rename to Assets/UnitySensorsROS/Samples/TF/TF.unity.meta