diff --git a/Assets/AWSIM/Scripts/NPCs/NPCs.cs b/Assets/AWSIM/Scripts/NPCs/NPCs.cs new file mode 100644 index 000000000..948a0869a --- /dev/null +++ b/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()); + } + } + +} \ No newline at end of file diff --git a/Assets/AWSIM/Scripts/NPCs/ROS2NPCPredictionController.cs b/Assets/AWSIM/Scripts/NPCs/ROS2NPCPredictionController.cs new file mode 100644 index 000000000..46adf1c4a --- /dev/null +++ b/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 Subscriber; + + PerceptionResultSensor objectSensor; + PerceptionResultSensor.OutputData outputData; + void Start() { + Subscriber = SimulatorROS2Node.CreateSubscription(subscribedTopic, myCallback, qoSSettings.GetQoSProfile()); + perceptionTrackingResultRos2Publisher = GetComponent(); + objectSensor = GetComponent(); + 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 path = new List(); + List rotation = new List(); + 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(Subscriber); + } + } +} \ No newline at end of file diff --git a/Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs b/Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs index d7466ef12..821a84106 100644 --- a/Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs +++ b/Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs @@ -9,7 +9,7 @@ namespace AWSIM /// NPC Vehicle class. /// Controlled by Position and Rotation. /// - 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,12 +324,20 @@ 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. @@ -322,6 +345,8 @@ void FixedUpdate() lastVelocity = velocity; lastEulerAnguleY = rigidbody.rotation.eulerAngles.y; lastSpeed = speed; + localLinearVelocity = transform.InverseTransformDirection(velocity); + localAngularVelocity = transform.InverseTransformDirection(angularVelocity); } void Reset() diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs index f1b4d043d..c7c4bd9f2 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs +++ b/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(); } diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleControlStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleControlStep.cs index 95370062c..b992743ee 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleControlStep.cs +++ b/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); + } } /// diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs index 46e989bab..d69ea95b5 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs +++ b/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]; } diff --git a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultSensor.cs b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultSensor.cs index 312db62d9..6563bcc08 100644 --- a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultSensor.cs +++ b/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; } /// @@ -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]; diff --git a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionTrackingResultRos2Publisher.cs b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionTrackingResultRos2Publisher.cs new file mode 100644 index 000000000..ded38b4bf --- /dev/null +++ b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionTrackingResultRos2Publisher.cs @@ -0,0 +1,206 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using System; +using System.Reflection; +using ROS2; + +namespace AWSIM +{ + /// + /// Convert the data output from PerceptionResultSensor to ROS2 msg and Publish. + /// + [RequireComponent(typeof(PerceptionResultSensor))] + public class PerceptionTrackingResultRos2Publisher : MonoBehaviour + { + /// + /// Topic name in DetectedObject msg. + /// + public string objectTopic = "/awsim/ground_truth/perception/object_recognition/tracking/objects"; + + /// + /// Object sensor frame id. + /// + public string frameId = "map"; + + /// + /// max distance that lidar can detect + /// + [Range(0, 200)] + public float maxDistance = 200f; + + /// + /// QoS settings. + /// + public QoSSettings qosSettings = new QoSSettings() + { + ReliabilityPolicy = ReliabilityPolicy.QOS_POLICY_RELIABILITY_RELIABLE, + DurabilityPolicy = DurabilityPolicy.QOS_POLICY_DURABILITY_VOLATILE, + HistoryPolicy = HistoryPolicy.QOS_POLICY_HISTORY_KEEP_LAST, + Depth = 1, + }; + + public Dictionary idToNpc = new Dictionary(); + + IPublisher objectPublisher; + autoware_perception_msgs.msg.TrackedObjects objectsMsg; + PerceptionResultSensor objectSensor; + + void Start() + { + // Get ObjectSensor component. + objectSensor = GetComponent(); + + // Set callback. + objectSensor.OnOutputData += Publish; + + // Create msg. + objectsMsg = new autoware_perception_msgs.msg.TrackedObjects(); + + // Create publisher. + var qos = qosSettings.GetQoSProfile(); + objectPublisher = SimulatorROS2Node.CreatePublisher(objectTopic, qos); + } + + void Publish(PerceptionResultSensor.OutputData outputData) + { + if (outputData == null || outputData.objects == null || outputData.origin == null) return; + var objectsList = new List(); + 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(); + if (NPC != null) + { + if (idToNpc.ContainsKey(BitConverter.ToString(NPC.uuid.Uuid)) == false){ + idToNpc.Add(BitConverter.ToString(NPC.uuid.Uuid), NPC); + } + } + var rb = detectedObject.rigidBody; + var dim = detectedObject.dimension; + var bou = detectedObject.bounds; + // Check if detectedObject.dimension and detectedObject.bounds are null + float distance = Vector3.Distance(outputData.origin.position, rb.transform.position); + + var obj = new autoware_perception_msgs.msg.TrackedObject(); + 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, NPC.uuid); + //add classification + var classification = new autoware_perception_msgs.msg.ObjectClassification(); + { + switch (detectedObject.classification) + { + case ObjectClassification.ObjectType.UNKNOWN: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.UNKNOWN; + break; + case ObjectClassification.ObjectType.CAR: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.CAR; + break; + case ObjectClassification.ObjectType.TRUCK: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.TRUCK; + break; + case ObjectClassification.ObjectType.BUS: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.BUS; + break; + case ObjectClassification.ObjectType.TRAILER: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.TRAILER; + break; + case ObjectClassification.ObjectType.MOTORCYCLE: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.MOTORCYCLE; + break; + case ObjectClassification.ObjectType.BICYCLE: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.BICYCLE; + break; + case ObjectClassification.ObjectType.PEDESTRIAN: + classification.Label = autoware_perception_msgs.msg.ObjectClassification.PEDESTRIAN; + break; + default: + Debug.LogWarning("Unknown classification type"); + break; + } + classification.Probability = 1.0f; + } + obj.Classification = new List { classification }.ToArray(); + + var kinematics = new autoware_perception_msgs.msg.TrackedObjectKinematics(); + // Add pose + { + var p = ROS2Utility.UnityToRosPosition(rb.transform.TransformPoint(rb.centerOfMass)); + var rosPosition = p + Environment.Instance.MgrsOffsetPosition; + + kinematics.Pose_with_covariance.Pose.Position.X = rosPosition.x; + kinematics.Pose_with_covariance.Pose.Position.Y = rosPosition.y; + kinematics.Pose_with_covariance.Pose.Position.Z = rosPosition.z; + + var r = ROS2Utility.UnityToRosRotation(rb.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; + } + // Add twist + { + var rosLinearVelocity = ROS2Utility.UnityToRosPosition(NPC.LinearVelocity); + kinematics.Twist_with_covariance.Twist.Linear.X = rosLinearVelocity.x; + kinematics.Twist_with_covariance.Twist.Linear.Y = rosLinearVelocity.y; + kinematics.Twist_with_covariance.Twist.Linear.Z = rosLinearVelocity.z; + + var rosAngularVelocity = ROS2Utility.UnityToRosAngularVelocity(NPC.AngularVelocity); + kinematics.Twist_with_covariance.Twist.Angular.X = rosAngularVelocity.x; + kinematics.Twist_with_covariance.Twist.Angular.Y = rosAngularVelocity.y; + kinematics.Twist_with_covariance.Twist.Angular.Z = rosAngularVelocity.z; + } + // Add covariance + { + kinematics.Orientation_availability = autoware_perception_msgs.msg.TrackedObjectKinematics.AVAILABLE; + const int size = 6; + for (int i = 0; i < size; i++) + { + kinematics.Pose_with_covariance.Covariance[i * size + i] = 1; + kinematics.Twist_with_covariance.Covariance[i * size + i] = 1; + } + } + obj.Kinematics = kinematics; + + // add shape and footprint + { + var shape = new autoware_perception_msgs.msg.Shape(); + shape.Type = autoware_perception_msgs.msg.Shape.BOUNDING_BOX; + shape.Dimensions.X = dim.x; + shape.Dimensions.Y = dim.y; + shape.Dimensions.Z = dim.z; + var footprints = new geometry_msgs.msg.Polygon(); + // Assuming Point32 has X, Y, Z properties + if (bou.Length > 0) + { + var point1 = new geometry_msgs.msg.Point32() { X = bou[0].x, Y = bou[0].y, Z = 0 }; + var point2 = new geometry_msgs.msg.Point32() { X = bou[1].x, Y = bou[1].y, Z = 0 }; + var point3 = new geometry_msgs.msg.Point32() { X = bou[2].x, Y = bou[2].y, Z = 0 }; + var point4 = new geometry_msgs.msg.Point32() { X = bou[3].x, Y = bou[3].y, Z = 0 }; + footprints.Points = new[] { point1, point2, point3, point4 }; + } + shape.Footprint = footprints; + obj.Shape = shape; + } + objectsList.Add(obj); + } + // Converts data output from ObjectSensor to ROS2 msg + objectsMsg.Objects = objectsList.ToArray(); + // Update msg header. + var header = objectsMsg as MessageWithHeader; + objectsMsg.Header.Stamp.Sec = outputData.seconds; + objectsMsg.Header.Stamp.Nanosec = outputData.nanoseconds; + objectsMsg.Header.Frame_id = frameId; + + // Publish to ROS2. + objectPublisher.Publish(objectsMsg); + } + + void OnDestroy() + { + SimulatorROS2Node.RemovePublisher(objectPublisher); + } + } +}