Change the method of controlling NPC Vehicles to map_based_prediction

Assets/AWSIM/Scripts/NPCs/NPC.cs

@@ -0,0 +1,30 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System;
+using System.Reflection;
+using ROS2;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// NPC Vehicle class.
+    /// Controlled by Position and Rotation.
+    /// </summary>
+    ///
+    public class NPC : MonoBehaviour
+    {
+        public unique_identifier_msgs.msg.UUID rosuuid;
+        public virtual Vector3 Velocity { get; }
+        public virtual Vector3 AngularVelocity { get; }
+
+        public void SetUUID(){
+        Guid guid = Guid.NewGuid();
+        rosuuid = new unique_identifier_msgs.msg.UUID();
+        PropertyInfo prop = rosuuid.GetType().GetProperty("Uuid", BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Instance | BindingFlags.Static);
+        prop.SetValue(rosuuid, guid.ToByteArray());
+
+    }
+    }
+
+}

Assets/AWSIM/Scripts/NPCs/Pedestrians/NPCPedestrian.cs

@@ -6,7 +6,7 @@ namespace AWSIM
     /// NPC pedestrian that is controlled in the scenario.
     /// </summary>
     [RequireComponent(typeof(Rigidbody), typeof(Animator))]
-    public class NPCPedestrian : MonoBehaviour
+    public class NPCPedestrian : NPC
     {
         [SerializeField] private new Rigidbody rigidbody;
         [SerializeField] private Transform referencePoint;
@@ -30,6 +30,17 @@ public class NPCPedestrian : MonoBehaviour
 
         private const string moveSpeedProperty = "moveSpeed";
         private const string rotateSpeedProperty = "rotateSpeed";
+        private Vector3 lastPosition;
+        private Vector3 velocity;
+        public override Vector3 Velocity => velocity;
+        private Vector3 angularVelocity;
+        public override Vector3 AngularVelocity => angularVelocity;
+        private Vector3 lastAngular;
+
+        private void Awake()
+        {
+            SetUUID();
+        }
 
         private void Update()
         {
@@ -41,6 +52,14 @@ private void Update()
             animator.SetFloat(rotateSpeedProperty, rigidbody.angularVelocity.magnitude);
         }
 
+        public void FixedUpdate()
+        {
+            velocity = (rigidbody.position - lastPosition) / Time.deltaTime;
+            lastPosition = rigidbody.position;
+            angularVelocity = (rigidbody.rotation.eulerAngles - lastAngular) / Time.deltaTime;
+            lastAngular = rigidbody.rotation.eulerAngles;
+        }
+
         private void Reset()
         {
             if (animator == null)

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 : NPC
     {
         public enum TurnSignalState
         {
@@ -188,6 +188,10 @@ public void Destroy()
         float wheelbase;        // m
         float acceleration;     // m/s^2
         Vector3 velocity;       // m/s
+        public override Vector3 Velocity => velocity;
+        Vector3 angularVelocity; // deg/s
+        Vector3 lastAngular;
+        public override Vector3 AngularVelocity => angularVelocity;
         float speed;            // m/s (forward only)
         float yawAngularSpeed;  // deg/s (yaw only)
 
@@ -198,6 +202,14 @@ public void Destroy()
 
         public Transform RigidBodyTransform => rigidbody.transform;
         public Transform TrailerTransform => trailer?.transform;
+        public bool outerControl = false; // if true, the vehicle is controlled by map_based_prediction
+
+        public Vector3 currentPosition;
+
+        [NonSerialized]
+        public Vector3 outerTargetPoint = new Vector3();
+        [NonSerialized]
+        public Quaternion outerRotation = new Quaternion();
 
         // Start is called before the first frame update
         void Awake()
@@ -211,6 +223,8 @@ void Awake()
             rigidbody.centerOfMass = transform.InverseTransformPoint(centerOfMass.position);
             lastPosition = rigidbody.position;
             wheelbase = axleSettings.GetWheelBase();
+
+            SetUUID();
         }
 
         // Update is called once per frame
@@ -303,10 +317,13 @@ bool IsRightTurniSignalOn()
         }
 
         void FixedUpdate()
-        {
+        {   
+
+            currentPosition = rigidbody.position;
             // Calculate physical states for visual update.
             // velocity & speed.
             velocity = (rigidbody.position - lastPosition) / Time.deltaTime;
+            angularVelocity = (rigidbody.rotation.eulerAngles - lastAngular) / Time.deltaTime;
             speed = Vector3.Dot(velocity, transform.forward);
 
             // accleration.
@@ -320,6 +337,7 @@ void FixedUpdate()
             // Cache current frame values.
             lastPosition = rigidbody.position;
             lastVelocity = velocity;
+            lastAngular = rigidbody.rotation.eulerAngles;
             lastEulerAnguleY = rigidbody.rotation.eulerAngles.y;
             lastSpeed = speed;
         }

Assets/AWSIM/Scripts/ROS/ObjectSubscriber.cs

@@ -0,0 +1,79 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using ROS2;
+
+namespace AWSIM {
+
+    public class ObjectSubscriber : MonoBehaviour {
+
+        //Get PerceptionResultRos2Publisher
+        public PerceptionResultRos2Publisher perceptionResultRos2Publisher;
+        QoSSettings qoSSettings = new QoSSettings();
+        string subscribedTopic = "/perception/object_recognition/objects";
+        ISubscription<autoware_auto_perception_msgs.msg.PredictedObjects> Subscriber;
+
+        void Start() {
+            Subscriber = SimulatorROS2Node.CreateSubscription<autoware_auto_perception_msgs.msg.PredictedObjects>(subscribedTopic, myCallback, qoSSettings.GetQoSProfile());
+            perceptionResultRos2Publisher = GetComponent<PerceptionResultRos2Publisher>();
+        }
+
+        void myCallback(autoware_auto_perception_msgs.msg.PredictedObjects receivedMsg){
+            var objects = receivedMsg.Objects;
+            //the first index represents the object path
+
+            // Get TimeStep
+            int rosSec = receivedMsg.Header.Stamp.Sec;
+            uint rosNanosec = receivedMsg.Header.Stamp.Nanosec;
+
+            int currentSec;
+            uint currentNanosec;
+
+            SimulatorROS2Node.TimeSource.GetTime(out currentSec, out currentNanosec);
+
+            for (var i = 0; i < objects.Length; i++){
+
+                List<Vector3> path = new List<Vector3>();
+                List<Quaternion> rotation = new List<Quaternion>();
+                var confidence = -1f;
+                var maxindex = 0;
+                for (var j = 0; j < objects[i].Kinematics.Predicted_paths.Length; j++){
+                    if (objects[i].Kinematics.Predicted_paths[j].Confidence > confidence){
+                        confidence = objects[i].Kinematics.Predicted_paths[j].Confidence;
+                        maxindex = j;
+                    }
+                }
+                uint deltaTime = objects[i].Kinematics.Predicted_paths[maxindex].Time_step.Nanosec;
+                int first_step = (int)((currentNanosec - rosNanosec) / deltaTime);
+                int end_step = first_step + 1;
+                float delta = (currentNanosec - rosNanosec) % deltaTime;
+
+                for (var j = 0; j < objects[i].Kinematics.Predicted_paths[maxindex].Path.Length; j++){
+                    var rosPosition = objects[i].Kinematics.Predicted_paths[maxindex].Path[j].Position;
+                    var unityPosition = ROS2Utility.RosMGRSToUnityPosition(rosPosition);
+                    var rosRotation = objects[i].Kinematics.Predicted_paths[maxindex].Path[j].Orientation;
+                    var unityRotation = ROS2Utility.RosToUnityRotation(rosRotation);
+                    path.Add(unityPosition);
+                    rotation.Add(unityRotation);
+                }
+                var uuid = BitConverter.ToString(objects[i].Object_id.Uuid);
+                if (perceptionResultRos2Publisher.id2npc[uuid].GetType().Name == "NPCVehicle"){
+                    var npcvehicle = (NPCVehicle)perceptionResultRos2Publisher.id2npc[uuid];
+                    var currentpostion = npcvehicle.currentPosition;
+                    var startPosition =  ROS2Utility.RosMGRSToUnityPosition(objects[i].Kinematics.Predicted_paths[maxindex].Path[first_step].Position);
+                    var endPosition =  ROS2Utility.RosMGRSToUnityPosition(objects[i].Kinematics.Predicted_paths[maxindex].Path[end_step].Position);
+                    npcvehicle.outerTargetPoint = Vector3.Slerp(startPosition, endPosition, delta);
+                    var startRotation = ROS2Utility.RosToUnityRotation(objects[i].Kinematics.Predicted_paths[maxindex].Path[first_step].Orientation);
+                    var endRotation = ROS2Utility.RosToUnityRotation(objects[i].Kinematics.Predicted_paths[maxindex].Path[end_step].Orientation);
+                    npcvehicle.outerRotation = Quaternion.Lerp(startRotation, endRotation, delta);
+                }
+
+            }
+        }
+
+        void OnDestroy(){
+            SimulatorROS2Node.RemoveSubscription<autoware_auto_perception_msgs.msg.PredictedObjects>(Subscriber);
+        }
+    }
+}

Assets/AWSIM/Scripts/ROS/ROS2Utility.cs

@@ -79,6 +79,17 @@ public static Vector3 RosMGRSToUnityPosition(geometry_msgs.msg.Point rosPosition
                                (float)(rosPosition.X - offset.x));
         }
 
+        public static geometry_msgs.msg.Point UnityToRosMGRSPosition(Vector3 unityPosition)
+        {
+            var offset = Environment.Instance.MgrsOffsetPosition;
+            return new geometry_msgs.msg.Point
+            {
+                X = unityPosition.z + offset.x,  // the Z axis in Unity is the X axis in ROS
+                Y = -unityPosition.x + offset.y, // the X axis in Unity is the Y axis in ROS
+                Z = unityPosition.y + offset.z   // the Y axis in Unity is the Z axis in ROS
+            };
+        }
+
         /// <summary>
         /// Convert position from ROS to Unity.
         /// </summary>

Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs

@@ -905,7 +905,6 @@ public void Execute()
                 {
                     if (GroundHitInfoArray[i].collider == null)
                         States[i].ShouldDespawn = true;
-
                     States[i].DistanceToFrontVehicle = ObstacleDistances[i];
                     States[i].IsTurning = IsTurnings[i];
                 }

Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs

@@ -38,8 +38,25 @@ private static void UpdateTargetPoint(NPCVehicleInternalState state)
         {
             if (state.ShouldDespawn || state.CurrentFollowingLane == null)
                 return;
-
-            state.TargetPoint = state.CurrentFollowingLane.Waypoints[state.WaypointIndex];
+            var vehicle = state.Vehicle;
+            if (vehicle.outerControl)
+            {   
+                // TODO: whether the target position should equal to the vehicle.outerTargetPoint or not?
+                var targetPosition = vehicle.outerTargetPoint + Quaternion.AngleAxis(state.Yaw, Vector3.up) * state.FrontCenterLocalPosition;
+                //var targetPosition = vehicle.outerTargetPoint;
+                if (Mathf.Abs((state.FrontCenterPosition - targetPosition).magnitude) > 0.01f)
+                {
+                    state.TargetPoint = targetPosition;
+                }
+                else
+                {
+                    state.TargetPoint = state.CurrentFollowingLane.Waypoints[state.WaypointIndex];
+                }
+            }
+            else
+            {
+                state.TargetPoint = state.CurrentFollowingLane.Waypoints[state.WaypointIndex];
+            }
         }
 
         /// <summary>