diff --git a/Assets/AWSIM/Scripts/NPCs/NPC.cs b/Assets/AWSIM/Scripts/NPCs/NPC.cs new file mode 100644 index 000000000..60e3c28d0 --- /dev/null +++ b/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 +{ + /// + /// NPC Vehicle class. + /// Controlled by Position and Rotation. + /// + /// + 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()); + + } + } + +} \ No newline at end of file diff --git a/Assets/AWSIM/Scripts/NPCs/Pedestrians/NPCPedestrian.cs b/Assets/AWSIM/Scripts/NPCs/Pedestrians/NPCPedestrian.cs index 7212197cc..bd53ffe1e 100644 --- a/Assets/AWSIM/Scripts/NPCs/Pedestrians/NPCPedestrian.cs +++ b/Assets/AWSIM/Scripts/NPCs/Pedestrians/NPCPedestrian.cs @@ -6,7 +6,7 @@ namespace AWSIM /// NPC pedestrian that is controlled in the scenario. /// [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) diff --git a/Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs b/Assets/AWSIM/Scripts/NPCs/Vehicles/NPCVehicle.cs index 896fb093a..1478d0b6a 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 : 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; } diff --git a/Assets/AWSIM/Scripts/ROS/ObjectSubscriber.cs b/Assets/AWSIM/Scripts/ROS/ObjectSubscriber.cs new file mode 100644 index 000000000..b53c44027 --- /dev/null +++ b/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 Subscriber; + + void Start() { + Subscriber = SimulatorROS2Node.CreateSubscription(subscribedTopic, myCallback, qoSSettings.GetQoSProfile()); + perceptionResultRos2Publisher = GetComponent(); + } + + 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 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; + } + } + 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(Subscriber); + } + } +} \ No newline at end of file diff --git a/Assets/AWSIM/Scripts/ROS/ROS2Utility.cs b/Assets/AWSIM/Scripts/ROS/ROS2Utility.cs index 4c92cdfa3..c139307c0 100644 --- a/Assets/AWSIM/Scripts/ROS/ROS2Utility.cs +++ b/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 + }; + } + /// /// Convert position from ROS to Unity. /// diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs index f1b4d043d..a13fbbd70 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs +++ b/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]; } diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs index 46e989bab..6c9f5e3a0 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs +++ b/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]; + } } /// diff --git a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultRos2Publisher.cs b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultRos2Publisher.cs index c30221269..fa9cf5406 100644 --- a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultRos2Publisher.cs +++ b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/PerceptionResultRos2Publisher.cs @@ -2,6 +2,7 @@ using System.Collections.Generic; using UnityEngine; using System; +using System.Reflection; using ROS2; namespace AWSIM @@ -15,7 +16,9 @@ public class PerceptionResultRos2Publisher : MonoBehaviour /// /// Topic name in DetectedObject msg. /// - public string objectTopic = "/awsim/ground_truth/perception/object_recognition/detection/objects"; + // public string objectTopic = "/awsim/ground_truth/perception/object_recognition/detection/objects"; + private string objectTopic = "/perception/object_recognition/tracking/objects"; + private string lightTopic = "/perception/traffic_light_recognition/traffic_signals"; /// /// Object sensor frame id. @@ -31,17 +34,25 @@ public class PerceptionResultRos2Publisher : MonoBehaviour /// /// QoS settings. /// + [NonSerialized] public QoSSettings qosSettings = new QoSSettings() { - ReliabilityPolicy = ReliabilityPolicy.QOS_POLICY_RELIABILITY_BEST_EFFORT, + ReliabilityPolicy = ReliabilityPolicy.QOS_POLICY_RELIABILITY_RELIABLE, DurabilityPolicy = DurabilityPolicy.QOS_POLICY_DURABILITY_VOLATILE, HistoryPolicy = HistoryPolicy.QOS_POLICY_HISTORY_KEEP_LAST, Depth = 1, }; - IPublisher objectPublisher; - autoware_auto_perception_msgs.msg.DetectedObjects objectsMsg; + private Dictionary obj2npc = new Dictionary(); + [NonSerialized] + public Dictionary id2npc = new Dictionary(); + + IPublisher objectPublisher; + IPublisher lightPublisher; + autoware_auto_perception_msgs.msg.TrackedObjects objectsMsg; PerceptionResultSensor objectSensor; + + private Dictionary trafficLights = new Dictionary(); void Start() { @@ -52,29 +63,62 @@ void Start() objectSensor.OnOutputData += Publish; // Create msg. - objectsMsg = new autoware_auto_perception_msgs.msg.DetectedObjects(); + objectsMsg = new autoware_auto_perception_msgs.msg.TrackedObjects(); // Create publisher. var qos = qosSettings.GetQoSProfile(); - objectPublisher = SimulatorROS2Node.CreatePublisher(objectTopic, qos); + objectPublisher = SimulatorROS2Node.CreatePublisher(objectTopic, qos); + lightPublisher = SimulatorROS2Node.CreatePublisher(lightTopic, qos); + + // Get ALl TrafficLights + GetLights(); + } + + void GetLights() { + var allTrafficLights = GameObject.FindObjectsOfType(); + var trafficLightObjects = FindObjectsOfType(); + for (int i = 0; i < trafficLightObjects.Length; i++) { + TrafficLightLaneletID laneletId = trafficLightObjects[i]; + GameObject obj = laneletId.gameObject; + TrafficLight tl = obj.GetComponent(); + if (tl != null && laneletId != null && trafficLights.ContainsKey(laneletId.wayID) == false){ + trafficLights.Add(laneletId.wayID, tl); + } + } } void Publish(PerceptionResultSensor.OutputData outputData) { if (outputData == null || outputData.objects == null || outputData.origin == null) return; - var objectsList = new List(); + var objectsList = new List(); foreach (var detectedObject in outputData.objects) { if(detectedObject ==null || detectedObject.rigidBody == null || detectedObject.dimension == null || detectedObject.bounds == null) continue; + + if (detectedObject.rigidBody.gameObject.GetComponent() != null) + { + if (obj2npc.ContainsKey(detectedObject.rigidBody.gameObject) == false ){ + obj2npc.Add(detectedObject.rigidBody.gameObject, detectedObject.rigidBody.gameObject.GetComponent()); + } + 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()); + } + } var rb = detectedObject.rigidBody; var dim = detectedObject.dimension; var bou = detectedObject.bounds; - // Check if detectedObject.dimension and detectedObject.bounds are null + // Check if detectedObject.dimension and detectedObject.bounds are null float distance = Vector3.Distance(outputData.origin.position, rb.transform.position); - if (distance > maxDistance) continue; + // if (distance > maxDistance) continue; - var obj = new autoware_auto_perception_msgs.msg.DetectedObject(); + //var obj = new autoware_auto_perception_msgs.msg.TrackedObject(); + var obj = new autoware_auto_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, obj2npc[detectedObject.rigidBody.gameObject].rosuuid); + //Debug.Log("UUID:" + BitConverter.ToString(obj.Object_id.Uuid)); + //add classification var classification = new autoware_auto_perception_msgs.msg.ObjectClassification(); { switch (detectedObject.classification) @@ -107,19 +151,21 @@ void Publish(PerceptionResultSensor.OutputData outputData) Debug.LogWarning("Unknown classification type"); break; } + + classification.Probability = 1.0f; } obj.Classification = new List{classification}.ToArray(); - var kinematics = new autoware_auto_perception_msgs.msg.DetectedObjectKinematics(); + var kinematics = new autoware_auto_perception_msgs.msg.TrackedObjectKinematics(); // Add pose { Vector3 relativePosition = rb.transform.position - outputData.origin.position; Vector3 transformedPosition = Quaternion.Inverse(outputData.origin.rotation) * relativePosition; - var p = ROS2Utility.UnityToRosPosition(transformedPosition); - 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 p = ROS2Utility.UnityToRosMGRSPosition(transformedPosition); + 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; // 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; @@ -128,21 +174,23 @@ void Publish(PerceptionResultSensor.OutputData outputData) kinematics.Pose_with_covariance.Pose.Orientation.W = r.w; } // Add twist - { - kinematics.Twist_with_covariance.Twist.Linear.X = rb.velocity.magnitude; + { + var velocity = obj2npc[detectedObject.rigidBody.gameObject].Velocity; + kinematics.Twist_with_covariance.Twist.Linear.X = Mathf.Sqrt(velocity.x * velocity.x + velocity.z * velocity.z); kinematics.Twist_with_covariance.Twist.Linear.Y = 0.0; kinematics.Twist_with_covariance.Twist.Linear.Z = 0.0; - var a = ROS2Utility.UnityToRosPosition(rb.angularVelocity); + var a = obj2npc[detectedObject.rigidBody.gameObject].AngularVelocity; + var ros_a = ROS2Utility.UnityToRosPosition(a); 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; + kinematics.Twist_with_covariance.Twist.Angular.Z = ros_a.z; } // Add covariance { - kinematics.Has_position_covariance = true; - kinematics.Orientation_availability = autoware_auto_perception_msgs.msg.DetectedObjectKinematics.AVAILABLE; - kinematics.Has_twist = true; - kinematics.Has_twist_covariance = true; + //kinematics.Has_position_covariance = true; + kinematics.Orientation_availability = autoware_auto_perception_msgs.msg.TrackedObjectKinematics.AVAILABLE; + //kinematics.Has_twist = true; + //kinematics.Has_twist_covariance = true; // Add covariance 6x6 const int size = 6; for (int i = 0; i < size; i++) @@ -179,15 +227,58 @@ void Publish(PerceptionResultSensor.OutputData outputData) // Update msg header. var header = objectsMsg as MessageWithHeader; SimulatorROS2Node.UpdateROSTimestamp(ref header); - objectsMsg.Header.Frame_id = frameId; + objectsMsg.Header.Frame_id = "map"; // Publish to ROS2. objectPublisher.Publish(objectsMsg); + //PubishLights(); + } + + void PubishLights() { + var lights = new List(); + + //TrafficSignal: lights.Signals -> (Map_primitive_id, Lights) + var trafficsignals = new List(); + + foreach (var x in trafficLights) { + var light = x.Value; + var bulbDatas = light.GetBulbData(); + var id = x.Key; + var tles = new List(); + var ts = new autoware_auto_perception_msgs.msg.TrafficSignal(); + ts.Map_primitive_id = (int)id; + foreach (var bulb in bulbDatas) { + var tl = new autoware_auto_perception_msgs.msg.TrafficLight(); + tl.Color = (Byte)(bulb.Color + 1); + //Debug.Log("type: " + tl.Color.GetType().Name); + + if (bulb.Type <= TrafficLight.BulbType.GREEN_BULB) { + tl.Shape = (Byte)5; + } else if (bulb.Type == TrafficLight.BulbType.CROSS_BULB) { + tl.Shape = 0; + } else { + tl.Shape = (Byte)(bulb.Type + 2); + } + tl.Status = (Byte)(bulb.Status + 13); + tl.Confidence = 1.0f; + tles.Add(tl); + } + ts.Lights = tles.ToArray(); + trafficsignals.Add(ts); + } + var lightsMsg = new autoware_auto_perception_msgs.msg.TrafficSignalArray(); + lightsMsg.Signals = trafficsignals.ToArray(); + var header = lightsMsg as MessageWithHeader; + SimulatorROS2Node.UpdateROSTimestamp(ref header); + lightsMsg.Header.Frame_id = "map"; + //lightPublisher.Publish(lightsMsg); + } void OnDestroy() { - SimulatorROS2Node.RemovePublisher(objectPublisher); + SimulatorROS2Node.RemovePublisher(objectPublisher); + SimulatorROS2Node.RemovePublisher(lightPublisher); } } }