feat: NPC-control by prediction module

Assets/AWSIM/Scripts/NPCs/NPCs.cs

@@ -0,0 +1,25 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System;
+using System.Reflection;
+using ROS2;
+
+namespace AWSIM
+{
+    public class NPCs : MonoBehaviour
+    {
+        public unique_identifier_msgs.msg.UUID  uuid;
+        public virtual Vector3 LinearVelocity { get; }
+        public virtual Vector3 AngularVelocity { get; }
+        public virtual float CurrentSpeed { get;}
+        public virtual Vector3 OuterTargetPoint { get; set;}
+        public void SetUUID(){
+            Guid guid = Guid.NewGuid();
+            uuid = new unique_identifier_msgs.msg.UUID();
+            PropertyInfo prop = uuid.GetType().GetProperty("Uuid", BindingFlags.Public | BindingFlags.NonPublic | BindingFlags.Instance | BindingFlags.Static);
+            prop.SetValue(uuid, guid.ToByteArray());
+        }
+    }
+
+}

Assets/AWSIM/Scripts/NPCs/ROS2NPCPredictionController.cs

@@ -0,0 +1,83 @@
+using UnityEngine;
+using ROS2;
+/**************/
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Reflection;
+/**********************/
+namespace AWSIM
+{
+    public class ROS2NPCPredictionController : MonoBehaviour
+    {
+        public PerceptionTrackingResultRos2Publisher perceptionTrackingResultRos2Publisher;
+        QoSSettings qoSSettings = new QoSSettings();
+        string subscribedTopic = "/awsim/perception/object_recognition/objects";
+        ISubscription<autoware_perception_msgs.msg.PredictedObjects> Subscriber;
+
+        PerceptionResultSensor objectSensor;
+        PerceptionResultSensor.OutputData outputData;
+        void Start() {
+            Subscriber = SimulatorROS2Node.CreateSubscription<autoware_perception_msgs.msg.PredictedObjects>(subscribedTopic, myCallback, qoSSettings.GetQoSProfile());
+            perceptionTrackingResultRos2Publisher = GetComponent<PerceptionTrackingResultRos2Publisher>();
+            objectSensor = GetComponent<PerceptionResultSensor>();
+            objectSensor.OnOutputData += Callback;
+        }
+
+        void myCallback(autoware_perception_msgs.msg.PredictedObjects receivedMsg){
+            var objects = receivedMsg.Objects;
+            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;
+                    }
+                }
+
+                var uuid = BitConverter.ToString(objects[i].Object_id.Uuid);
+                var npc = (NPCs)perceptionTrackingResultRos2Publisher.idToNpc[uuid];
+                var objectPosition = objects[i].Kinematics.Predicted_paths[maxindex].Path[0].Position;
+                int queueIndex = 0;
+                double distance = 0.0;
+                double nextDistance = 10.0;  
+
+                var deltaTime =(currentSec + currentNanosec/1e9) - (rosSec + rosNanosec/1e9);
+
+                uint predictionPointDeltaTime = objects[i].Kinematics.Predicted_paths[maxindex].Time_step.Nanosec;
+                int first_step = (int)(deltaTime / predictionPointDeltaTime);
+                int end_step = first_step + 1;
+
+                var endPositin = ROS2Utility.RosMGRSToUnityPosition(objects[i].Kinematics.Predicted_paths[maxindex].Path[end_step].Position);
+                var endRotation = ROS2Utility.RosToUnityRotation(objects[i].Kinematics.Predicted_paths[maxindex].Path[end_step].Orientation);
+
+                if (perceptionTrackingResultRos2Publisher.idToNpc[uuid].GetType().Name == "NPCVehicle"){
+                    var npcVehicle = (NPCVehicle)perceptionTrackingResultRos2Publisher.idToNpc[uuid];
+                    var direction = endRotation * Vector3.forward;
+                    npcVehicle.outerTargetPoint = endPositin + (direction * npcVehicle.Bounds.size.y);
+                    npcVehicle.outerTargetRotation = endRotation;
+                    npcVehicle.outerTargetPointTime = end_step*predictionPointDeltaTime - (float)deltaTime;
+                }
+            }
+        }
+
+        void Callback(PerceptionResultSensor.OutputData output){
+            outputData = output;
+        }
+
+        void OnDestroy(){
+            SimulatorROS2Node.RemoveSubscription<autoware_perception_msgs.msg.PredictedObjects>(Subscriber);
+        }
+    }
+}

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,25 @@ 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;
+
+        public bool outerControl { get; set; }
+        public float outerTargetPointTime { get; set; } = new float();
+        public Vector3 outerTargetPoint { get; set; } = new Vector3();
+        public Quaternion outerTargetRotation { get; set; } = new Quaternion();
+        public override float CurrentSpeed => speed;
+
         // Start is called before the first frame update
         void Awake()
         {
@@ -211,6 +224,8 @@ void Awake()
             rigidbody.centerOfMass = transform.InverseTransformPoint(centerOfMass.position);
             lastPosition = rigidbody.position;
             wheelbase = axleSettings.GetWheelBase();
+            SetUUID();
+            outerControl = false;
         }
 
         // Update is called once per frame
@@ -309,19 +324,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/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs

@@ -1104,6 +1104,16 @@ public void Execute(
                 States = states
             }.Execute();
 
+            foreach (var state in states)
+            {
+                if ((state.Vehicle.transform.position - egoTransform.position).magnitude <= 200F){
+                    state.Vehicle.outerControl = true;
+                } else {
+                    state.Vehicle.outerControl = false;
+                }
+            }
+
+
             Profiler.EndSample();
         }
 

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

@@ -73,6 +73,7 @@ private static void UpdateYawSpeed(NPCVehicleInternalState state, float deltaTim
         {
             // Steering the vehicle so that it heads toward the target point.
             var steeringDirection = state.TargetPoint - state.FrontCenterPosition;
+            var distance = Math.Sqrt(steeringDirection.x*steeringDirection.x + steeringDirection.y*steeringDirection.y);
             steeringDirection.y = 0f;
             var steeringAngle = Vector3.SignedAngle(state.Forward, steeringDirection, Vector3.up);
             var targetYawSpeed = steeringAngle * state.Speed * NPCVehicleConfig.YawSpeedMultiplier;
@@ -81,6 +82,15 @@ private static void UpdateYawSpeed(NPCVehicleInternalState state, float deltaTim
                 state.YawSpeed,
                 targetYawSpeed,
                 NPCVehicleConfig.YawSpeedLerpFactor * deltaTime);
+
+            var vehicle = state.Vehicle;
+            if(state.Vehicle.outerControl == true){
+                var rateTime = deltaTime/state.Vehicle.outerTargetPointTime;
+                state.YawSpeed = Mathf.Lerp(
+                state.YawSpeed,
+                targetYawSpeed,
+                NPCVehicleConfig.YawSpeedLerpFactor * rateTime);
+            }
         }
 
         /// <summary>

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

@@ -39,6 +39,14 @@ private static void UpdateTargetPoint(NPCVehicleInternalState state)
             if (state.ShouldDespawn || state.CurrentFollowingLane == null)
                 return;
 
+            if(state.Vehicle.outerControl == true){
+                state.TargetPoint = state.Vehicle.outerTargetPoint;
+            }
+            else
+            {
+                state.TargetPoint = state.CurrentFollowingLane.Waypoints[state.WaypointIndex];
+            }
+
             state.TargetPoint = state.CurrentFollowingLane.Waypoints[state.WaypointIndex];
         }
 

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];