Adjust GNSS publisher, Schenley scene

arbor-robotics · Apr 15, 2024 · 9b49216 · 9b49216
1 parent e23d524
commit 9b49216
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 31 deletions.
diff --git a/Assets/Scenes/Schenley.unity b/Assets/Scenes/Schenley.unity
@@ -38,7 +38,7 @@ RenderSettings:
   m_ReflectionIntensity: 1
   m_CustomReflection: {fileID: 0}
   m_Sun: {fileID: 0}
-  m_IndirectSpecularColor: {r: 2846.983, g: 2929.8167, b: 2976.344, a: 1}
+  m_IndirectSpecularColor: {r: 2928.646, g: 2977.9634, b: 2878.5002, a: 1}
   m_UseRadianceAmbientProbe: 0
 --- !u!157 &3
 LightmapSettings:
@@ -3480,52 +3480,67 @@ PrefabInstance:
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalPosition.x
-      value: 0
+      value: -0
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalPosition.y
-      value: 0
+      value: 0.167
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalPosition.z
-      value: 0.67499924
+      value: 0.676
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalRotation.w
-      value: 0.9990483
+      value: 0.99989337
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalRotation.x
-      value: -0.043619405
+      value: 0.002632724
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalRotation.y
-      value: 0
+      value: -0.014358977
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalRotation.z
-      value: 0
+      value: 0.0003765398
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalEulerAnglesHint.x
-      value: -5
+      value: 0.302
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalEulerAnglesHint.y
-      value: 0
+      value: -1.645
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
       propertyPath: m_LocalEulerAnglesHint.z
-      value: 0
+      value: 0.039
+      objectReference: {fileID: 0}
+    - target: {fileID: 7021877700071833489, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
+        type: 3}
+      propertyPath: angleMax
+      value: 0.785398
+      objectReference: {fileID: 0}
+    - target: {fileID: 7021877700071833489, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
+        type: 3}
+      propertyPath: angleMin
+      value: -0.785398
+      objectReference: {fileID: 0}
+    - target: {fileID: 7021877700071833489, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
+        type: 3}
+      propertyPath: rangeMax
+      value: 3
       objectReference: {fileID: 0}
     - target: {fileID: 7021877700071833489, guid: e9de4e56f7c42297d9f3cc3d57120a8f,
         type: 3}
@@ -3884,7 +3899,7 @@ PrefabInstance:
     - target: {fileID: 3004722648913154706, guid: d2f26ca743f1e5966b64d384acd6603c,
         type: 3}
       propertyPath: m_LocalPosition.y
-      value: 0.134
+      value: 0.123
       objectReference: {fileID: 0}
     - target: {fileID: 3004722648913154706, guid: d2f26ca743f1e5966b64d384acd6603c,
         type: 3}
@@ -5480,15 +5495,15 @@ PrefabInstance:
       objectReference: {fileID: 0}
     - target: {fileID: 490578, guid: e9a952fff0d5ee546bceadcd5743474e, type: 3}
       propertyPath: m_LocalPosition.x
-      value: 0
+      value: 426.6
       objectReference: {fileID: 0}
     - target: {fileID: 490578, guid: e9a952fff0d5ee546bceadcd5743474e, type: 3}
       propertyPath: m_LocalPosition.y
-      value: -9.56
+      value: 2.98
       objectReference: {fileID: 0}
     - target: {fileID: 490578, guid: e9a952fff0d5ee546bceadcd5743474e, type: 3}
       propertyPath: m_LocalPosition.z
-      value: 0
+      value: 660.1
       objectReference: {fileID: 0}
     - target: {fileID: 490578, guid: e9a952fff0d5ee546bceadcd5743474e, type: 3}
       propertyPath: m_LocalRotation.w
@@ -8403,6 +8418,11 @@ PrefabInstance:
       propertyPath: sidewaysStiffness
       value: 10
       objectReference: {fileID: 0}
+    - target: {fileID: 4586284701804823302, guid: 39eb98849209acb40807569d1d0136ea,
+        type: 3}
+      propertyPath: m_Enabled
+      value: 1
+      objectReference: {fileID: 0}
     - target: {fileID: 4586284701804823302, guid: 39eb98849209acb40807569d1d0136ea,
         type: 3}
       propertyPath: stalenessToleranceSeconds
@@ -8416,17 +8436,17 @@ PrefabInstance:
     - target: {fileID: 6104850889504349956, guid: 39eb98849209acb40807569d1d0136ea,
         type: 3}
       propertyPath: m_LocalPosition.x
-      value: 23.109812
+      value: 539.51
       objectReference: {fileID: 0}
     - target: {fileID: 6104850889504349956, guid: 39eb98849209acb40807569d1d0136ea,
         type: 3}
       propertyPath: m_LocalPosition.y
-      value: -9.52
+      value: 15.735
       objectReference: {fileID: 0}
     - target: {fileID: 6104850889504349956, guid: 39eb98849209acb40807569d1d0136ea,
         type: 3}
       propertyPath: m_LocalPosition.z
-      value: 37.39
+      value: 650
       objectReference: {fileID: 0}
     - target: {fileID: 6104850889504349956, guid: 39eb98849209acb40807569d1d0136ea,
         type: 3}
@@ -8509,6 +8529,10 @@ PrefabInstance:
         type: 3}
       insertIndex: -1
       addedObject: {fileID: 1893568651}
+    - targetCorrespondingSourceObject: {fileID: 4613237900554178101, guid: 39eb98849209acb40807569d1d0136ea,
+        type: 3}
+      insertIndex: -1
+      addedObject: {fileID: 1893568657}
     - targetCorrespondingSourceObject: {fileID: 4281249274067362048, guid: 39eb98849209acb40807569d1d0136ea,
         type: 3}
       insertIndex: -1
@@ -8532,6 +8556,22 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 75bee183affc7b381b189985847c4341, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
+--- !u!114 &1893568657
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 81356705}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 14909b68df2c38d2ea89c98a8f8147b5, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  nodeName: as_planting_service
+  plantingDelay: 2
+  seedlingPrefabs:
+  - {fileID: 6060788920606015911, guid: f5e80bd1bf6e910a9856d3589a1b6101, type: 3}
 --- !u!4 &1898999207 stripped
 Transform:
   m_CorrespondingSourceObject: {fileID: 8101256898395201125, guid: f4e4d8517c45a1a4390ceaa4221bd34e,

diff --git a/Assets/Scripts/ROS/GnssPublisher.cs b/Assets/Scripts/ROS/GnssPublisher.cs
@@ -19,7 +19,6 @@ public class GnssPublisher : MonoBehaviour
 private ROS2UnityComponent rosUnityComponent;
 private ROS2Node rosNode;
 private IPublisher<PoseWithCovarianceStamped> posePublisher;
-private IPublisher<NavSatFix> navSatFixPublisher;
 // TODO: Transform broadcaster
 
 PoseWithCovarianceStamped currentPose;
@@ -30,7 +29,6 @@ public class GnssPublisher : MonoBehaviour
 public string poseTopic;
 public string navSatFixTopic;
 public string mapFrameId = "map";
-public string originObjectTag = "MapFrameOrigin";         // An object with this tag will be treated as the map's origin
 
 void Start()
 {
@@ -39,7 +37,6 @@ void Start()
 	// Get the pose of our map's reference point. In our case,
 	// this is a statue. TODO: Parameterize this.
 	GameObject referenceObject = GameObject.Find("MapFrameOrigin");
-
 	mapOrigin = referenceObject.transform;
 }
 
@@ -52,15 +49,13 @@ void Update()
 			// Set up the node and publisher.
 			rosNode = rosUnityComponent.CreateNode(nodeName);
 			posePublisher = rosNode.CreateSensorPublisher<PoseWithCovarianceStamped>(poseTopic);
-			navSatFixPublisher = rosNode.CreateSensorPublisher<NavSatFix>(navSatFixTopic);
 
 			// Messages shouldn't be instantiated before the ROS node is created.
 			// https://github.com/RobotecAI/ros2-for-unity/issues/53#issuecomment-1418680445
 			currentPose = new PoseWithCovarianceStamped();
 		}
 
-		// Get the current pose
-		currentPose.Header = GetHeader();
+		currentPose.Header.Stamp = GetStamp();
 
 		// var diff = transform
 
@@ -69,9 +64,6 @@ void Update()
 		{
 			X = transform.position.x - mapOrigin.position.x, // Left is y in ROS, -x in Unity
 			Y = transform.position.z - mapOrigin.position.z, // Forward is x in ROS, z in Unity
-			// X = transform.position.x - 423.11,
-			// Y = transform.position.z - 661.07,
-			// Z = mapOrigin.position.y - transform.position.y  // Up is z in ROS, y in Unity
 			Z = 0f // TODO: Add support for elevation!!
 		};
 
@@ -89,10 +81,6 @@ void Update()
 			Z = currentOrientation.z
 		};
 
-		// Quaternion relative = Quaternion.Inverse(a) * b;
-
-		// TODO: Add covariance: currentPose.Pose.Covariance = ...
-
 		// Publish everything
 		posePublisher.Publish(currentPose);
 	}