Merge branch 'Field-Robotics-Japan:master' into master

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);

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()
         {

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()
         {

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<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();
+    }
 }

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

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<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

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
+}

Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/GPSPublisher.cs → Assets/UnitySensorsROS/Runtime/Scripts/Publishers/GPS/NMEAPublisher.cs

@@ -9,15 +9,15 @@ namespace UnitySensors.ROS
 {
     [RequireComponent(typeof(GPSSensor))]
     [ExecuteAlways]
-    public class GPSPublisher : Publisher<GPSSensor, Serializer>
+    public class NMEAPublisher : Publisher<GPSSensor, Serializer>
     {
         [SerializeField]
         private string _topicName = "gnss/raw_data";
         [SerializeField]
         private string _frame_id = "gnss_link";
 
         [SerializeField]
-        private GPSSerializer _serializer_gps;
+        private NMEASerializer _serializer_gps;
 
         protected override void Start()
         {

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<GPSSensor, Serializer>
+    {
+        [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<NavSatFixMsg>(_topicName);
+            _serializer_navsat.Init(_frame_id);
+        }
+
+        protected override void Publish(float time)
+        {
+            _serializer_navsat.Serialize(time, _sensor.coordinate);
+            _ros.Publish(_topicName, _serializer_navsat.msg);
+        }
+    }
+}
+