fix a bug that perception_replayer/perception_reproducer can't publis…

…h old 'autoware_auto_perception_msgs' in old rosbags.

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py

@@ -97,22 +97,14 @@ def on_timer(self):
             self.objects_pub.publish(objects_msg)
 
         # traffic signals
-        # temporary support old auto msgs
         if 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)
+            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:
-            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)
+            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

@@ -19,10 +19,9 @@
 from subprocess import check_output
 import time
 
-from autoware_perception_msgs.msg import DetectedObjects
-from autoware_perception_msgs.msg import PredictedObjects
-from autoware_perception_msgs.msg import TrackedObjects
-from autoware_perception_msgs.msg import TrafficLightGroupArray
+from autoware_perception_msgs.msg import DetectedObjects, PredictedObjects, TrackedObjects
+from autoware_perception_msgs.msg import TrafficLightGroupArray, TrafficLightGroup, TrafficLightElement
+
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from nav_msgs.msg import Odometry
@@ -79,6 +78,10 @@ def __init__(self, args, name):
             PoseStamped, "/planning/mission_planning/goal", 1
         )
 
+        self.traffic_signals_pub = self.create_publisher(
+            TrafficLightGroupArray, "/perception/traffic_light_recognition/traffic_signals", 1
+        )
+
         # load rosbag
         print("Stared loading rosbag")
         if os.path.isdir(args.bag):
@@ -89,10 +92,6 @@ def __init__(self, args, name):
             self.load_rosbag(args.bag)
         print("Ended loading rosbag")
 
-        self.traffic_signals_pub = self.create_publisher(
-            TrafficLightGroupArray, "/perception/traffic_light_recognition/traffic_signals", 1
-        )
-
         # wait for ready to publish/subscribe
         time.sleep(1.0)
 
@@ -123,10 +122,32 @@ def load_rosbag(self, rosbag2_path: str):
             msg_type = get_message(type_map[topic])
             msg = deserialize_message(data, msg_type)
             if topic == objects_topic:
+                if not isinstance(msg, self.objects_pub.msg_type):
+                    # convert old autoware_auto_perception_msgs to new autoware_perception_msgs
+                    new_msg = self.objects_pub.msg_type()
+                    for field in msg.__slots__:
+                        setattr(new_msg, field, getattr(msg, field))# it's unsafe because the elements inside the message are still the old type, but it works for now on.
+                    msg = new_msg
                 self.rosbag_objects_data.append((stamp, msg))
             if topic == ego_odom_topic:
                 self.rosbag_ego_odom_data.append((stamp, msg))
             if topic == traffic_signals_topic:
+                if not isinstance(msg, self.traffic_signals_pub.msg_type):
+                    # convert old TrafficSignalArray msg to new TrafficLightGroupArray msg.
+                    new_msg = self.traffic_signals_pub.msg_type()
+                    new_msg.stamp = msg.stamp
+                    for traffc_signal in msg.signals:
+                        traffic_lignt_group = TrafficLightGroup()
+                        traffic_lignt_group.traffic_light_group_id = traffc_signal.traffic_signal_id
+                        for traffic_signal_element in traffc_signal.elements:
+                            traffic_light_element = TrafficLightElement()
+                            traffic_light_element.color = traffic_signal_element.color
+                            traffic_light_element.shape = traffic_signal_element.shape
+                            traffic_light_element.status = traffic_signal_element.status
+                            traffic_light_element.confidence = traffic_signal_element.confidence
+                            traffic_lignt_group.elements.append(traffic_light_element)
+                        new_msg.traffic_light_groups.append(traffic_lignt_group)
+                    msg = new_msg
                 self.rosbag_traffic_signals_data.append((stamp, msg))
 
     def kill_online_perception_node(self):

planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py

@@ -106,24 +106,16 @@ def on_timer(self):
         self.recorded_ego_pub.publish(ego_odom[1])
 
         # traffic signals
-        # temporary support old auto msgs
         if 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)
+            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:
-            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)
+            self.prev_traffic_signals_msg.stamp = timestamp
+            self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
         self.stopwatch.toc("transform and publish")
 
+
         self.stopwatch.toc("total on_timer")
 
     def find_nearest_ego_odom_by_observation(self, ego_pose):