Field-Robotics-Japan · Autumn60 · Feb 18, 2024 · Feb 16, 2024 · Feb 16, 2024 · Feb 16, 2024
diff --git a/Assets/Test.unity b/Assets/Test.unity
diff --git a/Assets/UnitySensors/Runtime/Prefabs/GNSS/GeoCoordinateSystem.prefab b/Assets/UnitySensors/Runtime/Prefabs/GNSS/GeoCoordinateSystem.prefab
@@ -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
diff --git a/Assets/Test.unity.meta → ...fabs/GNSS/GeoCoordinateSystem.prefab.meta b/Assets/Test.unity.meta → ...fabs/GNSS/GeoCoordinateSystem.prefab.meta
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GNSSSensor.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GNSSSensor.cs
@@ -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();
         }

diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GeoCoordinateSystem.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GeoCoordinateSystem.cs
@@ -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);
+        }
+    }
+}
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GeoCoordinateSystem.cs.meta b/Assets/UnitySensors/Runtime/Scripts/Sensors/GNSS/GeoCoordinateSystem.cs.meta
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TF.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TF.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);
 

diff --git a/...ts/Utils/GeoCoordinates/GeoCoordinates.cs → ...pts/Utils/GeoCoordinates/GeoCoordinate.cs b/...ts/Utils/GeoCoordinates/GeoCoordinates.cs → ...pts/Utils/GeoCoordinates/GeoCoordinate.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)
         {

diff --git a/...ils/GeoCoordinates/GeoCoordinates.cs.meta → ...tils/GeoCoordinates/GeoCoordinate.cs.meta b/...ils/GeoCoordinates/GeoCoordinates.cs.meta → ...tils/GeoCoordinates/GeoCoordinate.cs.meta
diff --git a/...GeoCoordinates/GeoCoordinatesConverter.cs → .../GeoCoordinates/GeoCoordinateConverter.cs b/...GeoCoordinates/GeoCoordinatesConverter.cs → .../GeoCoordinates/GeoCoordinateConverter.cs
diff --git a/...ordinates/GeoCoordinatesConverter.cs.meta → ...oordinates/GeoCoordinateConverter.cs.meta b/...ordinates/GeoCoordinatesConverter.cs.meta → ...oordinates/GeoCoordinateConverter.cs.meta
diff --git a/Assets/UnitySensors/Samples/GNSS/Demo_GNSS.unity b/Assets/UnitySensors/Samples/GNSS/Demo_GNSS.unity
@@ -123,6 +123,18 @@ NavMeshSettings:
     debug:
       m_Flags: 0
   m_NavMeshData: {fileID: 0}
+--- !u!114 &2054003459 stripped
+MonoBehaviour:
+  m_CorrespondingSourceObject: {fileID: 6658069106543365611, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+    type: 3}
+  m_PrefabInstance: {fileID: 6658069108059838184}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 5f2116b7f84275c4d877f9ac14990716, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
 --- !u!1001 &6460962622283155330
 PrefabInstance:
   m_ObjectHideFlags: 0
@@ -135,6 +147,11 @@ PrefabInstance:
       propertyPath: m_Name
       value: GNSS
       objectReference: {fileID: 0}
+    - target: {fileID: 6460962621012238590, guid: a0db7cd58a019da4d84328d3f8cb748a,
+        type: 3}
+      propertyPath: _coordinateSystem
+      value: 
+      objectReference: {fileID: 2054003459}
     - target: {fileID: 6460962621012238591, guid: a0db7cd58a019da4d84328d3f8cb748a,
         type: 3}
       propertyPath: m_RootOrder
@@ -192,3 +209,72 @@ PrefabInstance:
       objectReference: {fileID: 0}
     m_RemovedComponents: []
   m_SourcePrefab: {fileID: 100100000, guid: a0db7cd58a019da4d84328d3f8cb748a, type: 3}
+--- !u!1001 &6658069108059838184
+PrefabInstance:
+  m_ObjectHideFlags: 0
+  serializedVersion: 2
+  m_Modification:
+    m_TransformParent: {fileID: 0}
+    m_Modifications:
+    - target: {fileID: 6658069106543365610, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_Name
+      value: GeoCoordinateSystem
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_RootOrder
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalPosition.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalPosition.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalPosition.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalRotation.w
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalRotation.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalRotation.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalRotation.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalEulerAnglesHint.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalEulerAnglesHint.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 6658069106543365612, guid: bfbdcabf0c3de544d92e2b824ec5a3a6,
+        type: 3}
+      propertyPath: m_LocalEulerAnglesHint.z
+      value: 0
+      objectReference: {fileID: 0}
+    m_RemovedComponents: []
+  m_SourcePrefab: {fileID: 100100000, guid: bfbdcabf0c3de544d92e2b824ec5a3a6, type: 3}
diff --git a/Assets/UnitySensors/package.json b/Assets/UnitySensors/package.json
@@ -1,6 +1,6 @@
 {
     "name": "com.frj.unity-sensors",
-    "version": "2.0.3",
+    "version": "2.0.4",
     "displayName": "UnitySensors",
     "description": "",
     "unity": "2021.3",

diff --git a/Assets/UnitySensorsROS/Runtime/Prefabs/GNSS/GNSS_ros.prefab b/Assets/UnitySensorsROS/Runtime/Prefabs/GNSS/GNSS_ros.prefab
@@ -10,7 +10,7 @@ GameObject:
   m_Component:
   - component: {fileID: 3192925013080293983}
   - component: {fileID: 3192925013080293982}
-  - component: {fileID: 3192925013080293981}
+  - component: {fileID: 3192925012673096858}
   m_Layer: 0
   m_Name: GNSS_ros
   m_TagString: Untagged
@@ -46,15 +46,12 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   _frequency: 10
-  _baseCoordinate:
-    latitude: 35.71020206575301
-    longitude: 139.81070039691542
-    altitude: 3
+  _coordinateSystem: {fileID: 0}
   _coordinate:
     latitude: 0
     longitude: 0
     altitude: 0
---- !u!114 &3192925013080293981
+--- !u!114 &3192925012673096858
 MonoBehaviour:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
@@ -63,13 +60,13 @@ MonoBehaviour:
   m_GameObject: {fileID: 3192925013080293980}
   m_Enabled: 1
   m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: 397c445553a6f3a40ac4d6892894ff7f, type: 3}
+  m_Script: {fileID: 11500000, guid: c11ec5602b9e12544b78d1d2a4bdd5c6, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
   _frequency: 10
-  _topicName: /navsat/fix
+  _topicName: navsat
   _serializer:
     _header:
-      _frame_id: /gnss_link
+      _frame_id: map
     _status: 1
     _service: 0