diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index 5f9e95759..c33b5dfe5 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/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(): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 5f83b9502..9fa8890d0 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/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): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index b2b6a3c0e..1a914f28f 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/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):