diff --git a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/AutowarePerceptionResultRos2Publisher.cs b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/AutowarePerceptionResultRos2Publisher.cs index d0589c280..34cf3b55e 100644 --- a/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/AutowarePerceptionResultRos2Publisher.cs +++ b/Assets/AWSIM/Scripts/Sensors/PerceptionResultSensor/AutowarePerceptionResultRos2Publisher.cs @@ -17,7 +17,6 @@ public class AutowarePerceptionResultRos2Publisher : MonoBehaviour /// Topic name in DetectedObject msg. /// public string objectTopic = "/perception/object_recognition/tracking/objects"; - // private string trafficLightTopic = "/perception/traffic_light_recognition/traffic_signals"; /// /// Object sensor frame id. @@ -46,12 +45,9 @@ public class AutowarePerceptionResultRos2Publisher : MonoBehaviour public Dictionary id2npc = new Dictionary(); IPublisher objectPublisher; - // IPublisher lightPublisher; autoware_perception_msgs.msg.TrackedObjects objectsMsg; PerceptionResultSensor objectSensor; - // private Dictionary trafficLights = new Dictionary(); - void Start() { // Get ObjectSensor component. @@ -66,25 +62,8 @@ void Start() // Create publisher. var qos = qosSettings.GetQoSProfile(); objectPublisher = SimulatorROS2Node.CreatePublisher(objectTopic, qos); - // lightPublisher = SimulatorROS2Node.CreatePublisher(trafficLightTopic, 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; @@ -107,14 +86,12 @@ void Publish(PerceptionResultSensor.OutputData outputData) var bou = detectedObject.bounds; // Check if detectedObject.dimension and detectedObject.bounds are null float distance = Vector3.Distance(outputData.origin.position, rb.transform.position); - // if (distance > maxDistance) continue; 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, obj2npc[detectedObject.rigidBody.gameObject].rosuuid); - //Debug.Log("UUID:" + BitConverter.ToString(obj.Object_id.Uuid)); //add classification var classification = new autoware_perception_msgs.msg.ObjectClassification(); { @@ -182,11 +159,7 @@ void Publish(PerceptionResultSensor.OutputData outputData) } // Add covariance { - // kinematics.Has_position_covariance = true; kinematics.Orientation_availability = autoware_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++) { @@ -229,50 +202,9 @@ void Publish(PerceptionResultSensor.OutputData outputData) objectPublisher.Publish(objectsMsg); } - // 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_perception_msgs.msg.TrafficSignal(); - // ts.Map_primitive_id = (int)id; - // foreach (var bulb in bulbDatas) { - // var tl = new autoware_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_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(lightPublisher); } } -} \ No newline at end of file +}