feat(planning_debug_tools): support old auto TrafficSignalArray (auto…

…warefoundation#4910)

* feat(planning_debug_tools): support old auto TrafficSignalArray

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* use is_auto flag

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py

@@ -92,13 +92,22 @@ def on_timer(self):
             self.objects_pub.publish(objects_msg)
 
         # traffic signals
+        # temporary support old auto msgs
         if traffic_signals_msg:
-            traffic_signals_msg.stamp = timestamp
-            self.traffic_signals_pub.publish(traffic_signals_msg)
+            if self.is_auto_traffic_signals:
+                traffic_signals_msg.header.stamp = timestamp
+                self.auto_traffic_signals_pub.publish(traffic_signals_msg)
+            else:
+                traffic_signals_msg.stamp = timestamp
+                self.traffic_signals_pub.publish(traffic_signals_msg)
             self.prev_traffic_signals_msg = traffic_signals_msg
         elif self.prev_traffic_signals_msg:
-            self.prev_traffic_signals_msg.stamp = timestamp
-            self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
+            if self.is_auto_traffic_signals:
+                self.prev_traffic_signals_msg.header.stamp = timestamp
+                self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg)
+            else:
+                self.prev_traffic_signals_msg.stamp = timestamp
+                self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
 
     def onPushed(self, event):
         if self.widget.button.isChecked():

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py

@@ -22,6 +22,7 @@
 from autoware_auto_perception_msgs.msg import DetectedObjects
 from autoware_auto_perception_msgs.msg import PredictedObjects
 from autoware_auto_perception_msgs.msg import TrackedObjects
+from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray
 from autoware_perception_msgs.msg import TrafficSignalArray
 from nav_msgs.msg import Odometry
 import psutil
@@ -65,9 +66,6 @@ def __init__(self, args, name):
         self.pointcloud_pub = self.create_publisher(
             PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1
         )
-        self.traffic_signals_pub = self.create_publisher(
-            TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1
-        )
 
         # load rosbag
         print("Stared loading rosbag")
@@ -78,6 +76,16 @@ def __init__(self, args, name):
             self.load_rosbag(args.bag)
         print("Ended loading rosbag")
 
+        # temporary support old auto msgs
+        if self.is_auto_traffic_signals:
+            self.auto_traffic_signals_pub = self.create_publisher(
+                AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1
+            )
+        else:
+            self.traffic_signals_pub = self.create_publisher(
+                TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1
+            )
+
         # wait for ready to publish/subscribe
         time.sleep(1.0)
 
@@ -113,6 +121,9 @@ def load_rosbag(self, rosbag2_path: str):
                 self.rosbag_ego_odom_data.append((stamp, msg))
             if topic == traffic_signals_topic:
                 self.rosbag_traffic_signals_data.append((stamp, msg))
+                self.is_auto_traffic_signals = (
+                    "autoware_auto_perception_msgs" in type(msg).__module__
+                )
 
     def kill_online_perception_node(self):
         # kill node if required

planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py

@@ -66,13 +66,22 @@ def on_timer(self):
             self.objects_pub.publish(objects_msg)
 
         # traffic signals
+        # temporary support old auto msgs
         if traffic_signals_msg:
-            traffic_signals_msg.stamp = timestamp
-            self.traffic_signals_pub.publish(traffic_signals_msg)
+            if self.is_auto_traffic_signals:
+                traffic_signals_msg.header.stamp = timestamp
+                self.auto_traffic_signals_pub.publish(traffic_signals_msg)
+            else:
+                traffic_signals_msg.stamp = timestamp
+                self.traffic_signals_pub.publish(traffic_signals_msg)
             self.prev_traffic_signals_msg = traffic_signals_msg
         elif self.prev_traffic_signals_msg:
-            self.prev_traffic_signals_msg.stamp = timestamp
-            self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
+            if self.is_auto_traffic_signals:
+                self.prev_traffic_signals_msg.header.stamp = timestamp
+                self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg)
+            else:
+                self.prev_traffic_signals_msg.stamp = timestamp
+                self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
 
     def find_nearest_ego_odom_by_observation(self, ego_pose):
         if self.ego_pose_idx: