Improve/perception replayer

.gitignore

@@ -1 +1,2 @@
-*.py[cod]
+*.pyc
+__pycache__/

planning/planning_debug_tools/CMakeLists.txt

@@ -52,6 +52,7 @@ install(PROGRAMS
   scripts/trajectory_visualizer.py
   scripts/closest_velocity_checker.py
   scripts/perception_replayer/perception_reproducer.py
+  scripts/perception_replayer/perception_reproducer_v2.py
   scripts/perception_replayer/perception_replayer.py
   scripts/update_logger_level.sh
   DESTINATION lib/${PROJECT_NAME}

planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py

@@ -97,22 +97,13 @@ 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

@@ -22,6 +22,8 @@
 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 TrafficLightElement
+from autoware_perception_msgs.msg import TrafficLightGroup
 from autoware_perception_msgs.msg import TrafficLightGroupArray
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import PoseWithCovarianceStamped
@@ -32,6 +34,7 @@
 from rosbag2_py import StorageFilter
 from rosidl_runtime_py.utilities import get_message
 from sensor_msgs.msg import PointCloud2
+from utils import get_starting_time
 from utils import open_reader
 
 
@@ -79,20 +82,24 @@
             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):
-            for bag_file in sorted(os.listdir(args.bag)):
-                if bag_file.endswith(self.args.rosbag_format):
-                    self.load_rosbag(args.bag + "/" + bag_file)
+            bags = [
+                os.path.join(args.bag, base_name)
+                for base_name in os.listdir(args.bag)
+                if base_name.endswith(args.rosbag_format)
+            ]
+            for bag_file in sorted(bags, key=get_starting_time):
+                self.load_rosbag(bag_file)
         else:
             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 +130,37 @@
             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,22 +106,13 @@ 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")