feat NPC-tracking-GT publisher

Signed-off-by: kazushi67 <[email protected]>

Assets/AWSIM/Scripts/NPCs/NPCs.cs

@@ -0,0 +1,21 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System;
+using System.Reflection;
+using ROS2;
+
+namespace AWSIM
+{
+    public class NPCs : MonoBehaviour
+    {
+        public string uuid;
+        public virtual Vector3 LinearVelocity { get; }
+        public virtual Vector3 AngularVelocity { get; }
+        public void SetUUID(){
+            Guid guid = Guid.NewGuid();
+            uuid = guid.ToString();
+        }
+    }
+
+}

Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs

@@ -9,7 +9,7 @@ namespace AWSIM
     /// NPC Vehicle class.
     /// Controlled by Position and Rotation.
     /// </summary>
-    public class NPCVehicle : MonoBehaviour
+    public class NPCVehicle : NPCs
     {
         public enum TurnSignalState
         {
@@ -193,12 +193,19 @@ public void Destroy()
 
         Vector3 lastVelocity;
         Vector3 lastPosition;
+        QuaternionD lastRotation;
         float lastEulerAnguleY;
         float lastSpeed;
 
         public Transform RigidBodyTransform => rigidbody.transform;
         public Transform TrailerTransform => trailer?.transform;
 
+        Vector3 localLinearVelocity;
+        public override Vector3 LinearVelocity => localLinearVelocity;
+
+        Vector3 localAngularVelocity;
+        public override Vector3 AngularVelocity => localAngularVelocity;
+
         // Start is called before the first frame update
         void Awake()
         {
@@ -211,6 +218,7 @@ void Awake()
             rigidbody.centerOfMass = transform.InverseTransformPoint(centerOfMass.position);
             lastPosition = rigidbody.position;
             wheelbase = axleSettings.GetWheelBase();
+            SetUUID();
         }
 
         // Update is called once per frame
@@ -309,19 +317,29 @@ void FixedUpdate()
             velocity = (rigidbody.position - lastPosition) / Time.deltaTime;
             speed = Vector3.Dot(velocity, transform.forward);
 
+            // angular velocity
+            var currentRotation = new QuaternionD(rigidbody.rotation);
+            var deltaRotation = currentRotation * QuaternionD.Inverse(lastRotation);
+            deltaRotation.ToAngleAxis(out var angle, out var axis);
+            var angularVelocity = ((float)angle * axis) / Time.deltaTime;
+
             // accleration.
             acceleration = (speed - lastSpeed) / Time.deltaTime;
 
             // yaw angular speed.
             yawAngularSpeed = (rigidbody.rotation.eulerAngles.y - lastEulerAnguleY) / Time.deltaTime;
 
+            localLinearVelocity = (transform.InverseTransformDirection(rigidbody.position - lastPosition)) / Time.deltaTime;
+
             // TODO: set WheelCollider steer angle?
 
             // Cache current frame values.
             lastPosition = rigidbody.position;
             lastVelocity = velocity;
             lastEulerAnguleY = rigidbody.rotation.eulerAngles.y;
             lastSpeed = speed;
+            localLinearVelocity = transform.InverseTransformDirection(velocity);
+            localAngularVelocity = transform.InverseTransformDirection(angularVelocity);
         }
 
         void Reset()

Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultSensor.cs

@@ -30,6 +30,8 @@ public class OutputData
         {
             public DetectedObject[] objects;
             public Transform origin;
+            public int seconds;
+            public uint nanoseconds;
         }
 
         /// <summary>
@@ -92,6 +94,7 @@ Vector2[] GenerateFootprint(Vector3 dimensions, Rigidbody vehicleRb)
         void CreateDetectedObjectData()
         {
             outputData.objects = new DetectedObject[cachedObjectsWithClassification.Length];
+            AWSIM.SimulatorROS2Node.TimeSource.GetTime(out outputData.seconds, out outputData.nanoseconds);
             for (int i = 0; i < cachedObjectsWithClassification.Length; i++)
             {
                 ObjectClassification obj = cachedObjectsWithClassification[i];

Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionTrackingResultRos2Publisher.cs

@@ -40,9 +40,7 @@ public class AutowarePerceptionResultRos2Publisher : MonoBehaviour
             Depth = 1,
         };
 
-        private Dictionary<GameObject, NPC> obj2npc = new Dictionary<GameObject, NPC>();
-        [NonSerialized]
-        public Dictionary<string, NPC> id2npc = new Dictionary<string, NPC>();
+        public Dictionary<string, NPCs> idToNpc = new Dictionary<string, NPCs>();
 
         IPublisher<autoware_perception_msgs.msg.TrackedObjects> objectPublisher;
         autoware_perception_msgs.msg.TrackedObjects objectsMsg;
@@ -71,14 +69,11 @@ void Publish(PerceptionResultSensor.OutputData outputData)
             foreach (var detectedObject in outputData.objects)
             {
                 if (detectedObject == null || detectedObject.rigidBody == null || detectedObject.dimension == null || detectedObject.bounds == null) continue;
-                var NPC = detectedObject.rigidBody.gameObject.GetComponent<NPC>();
+                var NPC = detectedObject.rigidBody.gameObject.GetComponent<NPCs>();
                 if (NPC != null)
                 {
-                    if (obj2npc.ContainsKey(detectedObject.rigidBody.gameObject) == false ){
-                        obj2npc.Add(detectedObject.rigidBody.gameObject, detectedObject.rigidBody.gameObject.GetComponent<NPC>());
-                    }
-                    if (id2npc.ContainsKey(BitConverter.ToString(obj2npc[detectedObject.rigidBody.gameObject].rosuuid.Uuid)) == false){
-                        id2npc.Add(BitConverter.ToString(obj2npc[detectedObject.rigidBody.gameObject].rosuuid.Uuid), detectedObject.rigidBody.gameObject.GetComponent<NPC>());
+                    if (idToNpc.ContainsKey(NPC.uuid) == false){
+                        idToNpc.Add(NPC.uuid, NPC);
                     }
                 }
                 var rb = detectedObject.rigidBody;
@@ -91,7 +86,7 @@ void Publish(PerceptionResultSensor.OutputData outputData)
                 obj.Existence_probability = 1.0f;
                 // add UUID 
                 PropertyInfo property = obj.GetType().GetProperty("Object_id", BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Instance | BindingFlags.Static);
-                property.SetValue(obj, obj2npc[detectedObject.rigidBody.gameObject].rosuuid);
+                property.SetValue(obj, NPC.uuid);
                 //add classification
                 var classification = new autoware_perception_msgs.msg.ObjectClassification();
                 {