Merge pull request #145 from Field-Robotics-Japan/develop

Release new version

Author: Autumn60

Assets/Test.unity

@@ -0,0 +1,50 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!1 &6658069106543365610
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 6658069106543365612}
+  - component: {fileID: 6658069106543365611}
+  m_Layer: 0
+  m_Name: GeoCoordinateSystem
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &6658069106543365612
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 6658069106543365610}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 0
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &6658069106543365611
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 6658069106543365610}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 5f2116b7f84275c4d877f9ac14990716, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  _coordinate:
+    latitude: 35.71020206575301
+    longitude: 139.81070039691542
+    altitude: 3

Assets/UnitySensors/Runtime/Prefabs/GNSS/GeoCoordinateSystem.prefab

@@ -10,7 +10,7 @@ namespace UnitySensors.Sensor.GNSS
     public class GNSSSensor : UnitySensor
     {
         [SerializeField]
-        private GeoCoordinate _baseCoordinate = new GeoCoordinate(35.71020206575301, 139.81070039691542, 3.0f);
+        private GeoCoordinateSystem _coordinateSystem;
 
         private Transform _transform;
         private GeoCoordinateConverter _gcc;
@@ -22,16 +22,11 @@ public class GNSSSensor : UnitySensor
         protected override void Init()
         {
             _transform = this.transform;
-            _gcc = new GeoCoordinateConverter(_baseCoordinate.latitude, _baseCoordinate.longitude);
-            _coordinate = new GeoCoordinate(_baseCoordinate.latitude, _baseCoordinate.longitude, _baseCoordinate.altitude);
         }
 
         protected override void UpdateSensor()
         {
-            Vector3 position = _transform.position;
-            (_coordinate.latitude, _coordinate.longitude) = _gcc.XZ2LatLon(position.x, position.z);
-            _coordinate.altitude = _baseCoordinate.altitude + position.y;
-            
+            _coordinate = _coordinateSystem.GetCoordinate(_transform.position);
             if (onSensorUpdated != null)
                 onSensorUpdated.Invoke();
         }

Assets/Test.unity.meta …abs/GNSS/GeoCoordinateSystem.prefab.metaAssets/Test.unity.meta renamed to Assets/UnitySensors/Runtime/Prefabs/GNSS/GeoCoordinateSystem.prefab.meta Assets/Test.unity.meta renamed to Assets/UnitySensors/Runtime/Prefabs/GNSS/GeoCoordinateSystem.prefab.meta

@@ -0,0 +1,31 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+using UnitySensors.Utils.GeoCoordinate;
+
+namespace UnitySensors.Sensor.GNSS
+{
+    public class GeoCoordinateSystem : MonoBehaviour
+    {
+        [SerializeField]
+        private GeoCoordinate _coordinate = new GeoCoordinate(35.71020206575301, 139.81070039691542, 3.0f);
+
+        private Transform _transform;
+        private GeoCoordinateConverter _converter;
+
+        private void Awake()
+        {
+            _transform = this.transform;
+            _converter = new GeoCoordinateConverter(_coordinate.latitude, _coordinate.longitude);
+        }
+
+        public GeoCoordinate GetCoordinate(Vector3 worldPosition)
+        {
+            Vector3 localPosition = _transform.InverseTransformPoint(worldPosition);
+            double latitude, longitude;
+            (latitude, longitude) = _converter.XZ2LatLon(localPosition.x, localPosition.z);
+            return new GeoCoordinate(latitude, longitude, localPosition.y + _coordinate.altitude);
+        }
+    }
+}

Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GNSSSensor.cs

@@ -55,6 +55,15 @@ public TFData[] GetTFData(string frame_id_parent, Matrix4x4 worldToLocalMatrix,
             tfData_self.frame_id_parent = frame_id_parent;
             tfData_self.frame_id_child = _frame_id;
             tfData_self.position = (Vector3)(worldToLocalMatrix * new Vector4(_transform.position.x, _transform.position.y, _transform.position.z, 1.0f));
+            Vector3 localScale = _transform.localScale;
+            Vector3 lossyScale = _transform.lossyScale;
+            Vector3 scaleVector = new Vector3()
+            {
+                x = localScale.x != 0 ? lossyScale.x / localScale.x : 0,
+                y = localScale.y != 0 ? lossyScale.y / localScale.y : 0,
+                z = localScale.z != 0 ? lossyScale.z / localScale.z : 0
+            };
+            tfData_self.position.Scale(scaleVector);
             tfData_self.rotation = worldToLocalQuaternion * _transform.rotation;
             tfData.Add(tfData_self);
 

Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GeoCoordinateSystem.cs

@@ -1,7 +1,7 @@
 namespace UnitySensors.Utils.GeoCoordinate
 {
     [System.Serializable]
-    public class GeoCoordinate
+    public struct GeoCoordinate
     {
         public GeoCoordinate(double lat, double lon, double alt)
         {