fix: orientation

tier4 · Oct 4, 2023 · 2292c21 · 2292c21
1 parent 26b0c0b
commit 2292c21
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 15 deletions.
diff --git a/Assets/AWSIM/Scripts/Sensors/ObjectSensor/ObjectRos2Publisher.cs b/Assets/AWSIM/Scripts/Sensors/ObjectSensor/ObjectRos2Publisher.cs
@@ -120,21 +120,23 @@ void Publish(ObjectSensor.OutputData outputData)
                     kinematics.Pose_with_covariance.Pose.Position.X = p.x;
                     kinematics.Pose_with_covariance.Pose.Position.Y = p.y;
                     kinematics.Pose_with_covariance.Pose.Position.Z = p.z;
-                    var r = ROS2Utility.UnityToRosRotation(Quaternion.Inverse(outputData.origin.rotation)*rb.transform.rotation);
+                    // add initial rotation of object
+                    var r = ROS2Utility.UnityToRosRotation(Quaternion.Inverse(outputData.origin.rotation) *rb.transform.rotation);
                     kinematics.Pose_with_covariance.Pose.Orientation.X = r.x;
                     kinematics.Pose_with_covariance.Pose.Orientation.Y = r.y;
                     kinematics.Pose_with_covariance.Pose.Orientation.Z = r.z;
+                    kinematics.Pose_with_covariance.Pose.Orientation.W = r.w;
+                    Debug.Log(rb.transform.rotation);
                 }
                 // Add twist
                 {
-                    var t = ROS2Utility.UnityToRosPosition(rb.velocity);
-                    kinematics.Twist_with_covariance.Twist.Linear.X = t.x;
-                    kinematics.Twist_with_covariance.Twist.Linear.Y = t.y;
-                    kinematics.Twist_with_covariance.Twist.Linear.Z = t.z;
+                    kinematics.Twist_with_covariance.Twist.Linear.X = rb.velocity.magnitude;
+                    kinematics.Twist_with_covariance.Twist.Linear.Y = 0.0;
+                    kinematics.Twist_with_covariance.Twist.Linear.Z = 0.0;
                     var a = ROS2Utility.UnityToRosPosition(rb.angularVelocity);
-                    kinematics.Twist_with_covariance.Twist.Angular.X = t.x;
-                    kinematics.Twist_with_covariance.Twist.Angular.Y = t.y;
-                    kinematics.Twist_with_covariance.Twist.Angular.Z = t.z;
+                    kinematics.Twist_with_covariance.Twist.Angular.X = 0.0;
+                    kinematics.Twist_with_covariance.Twist.Angular.Y = 0.0;
+                    kinematics.Twist_with_covariance.Twist.Angular.Z = a.z;
                 }
                 // Add covariance
                 {

diff --git a/Assets/AWSIM/Scripts/Sensors/ObjectSensor/ObjectSensor.cs b/Assets/AWSIM/Scripts/Sensors/ObjectSensor/ObjectSensor.cs
@@ -53,7 +53,8 @@ public class OutputData
         float timer = 0;
         public OutputData outputData = new OutputData();
         private List<Classification> filteredObjects = new List<Classification>();
-        private Classification[] cachedObjectsWithClassification;
+        public Classification[] cachedObjectsWithClassification;
+        private bool manuallyCached = true;
 
         // This method generates a footprint for the vehicle based on its dimensions, position, and rotation.
         Vector2[] GenerateFootprint(Vector3 dimensions, Rigidbody vehicleRb)
@@ -123,7 +124,12 @@ void CreateDetectedObjectData(){
         void Start()
         {
             outputData.origin = this.transform;
-            cachedObjectsWithClassification = FindObjectsOfType<Classification>();
+            // Check if cachedObjectsWithClassification is empty
+            if(cachedObjectsWithClassification == null || cachedObjectsWithClassification.Length == 0)
+            {
+                cachedObjectsWithClassification = FindObjectsOfType<Classification>();
+                manuallyCached = false;
+            }
             CreateDetectedObjectData();
         }
 
@@ -137,18 +143,19 @@ void FixedUpdate()
                 return;
             timer = 0;
             outputData.origin = this.transform;
-            var currentObjectsWithClassification = FindObjectsOfType<Classification>();
-            if (!Enumerable.SequenceEqual(cachedObjectsWithClassification, currentObjectsWithClassification)) {
-                cachedObjectsWithClassification = currentObjectsWithClassification;
-                CreateDetectedObjectData();
+            if(!manuallyCached){
+                var currentObjectsWithClassification = FindObjectsOfType<Classification>();
+                if (!Enumerable.SequenceEqual(cachedObjectsWithClassification, currentObjectsWithClassification)) {
+                    cachedObjectsWithClassification = currentObjectsWithClassification;
+                    CreateDetectedObjectData();
+                }
             }
             for (int i = 0; i < cachedObjectsWithClassification.Length; i++)
             {
                 var o = outputData.objects[i];
                 if(o == null) continue;
                 outputData.objects[i].bounds = GenerateFootprint(o.dimension,o.rigidBody);
             }
-
             // Calls registered callbacks
             OnOutputData.Invoke(outputData);
         }