From 5a33a2e0954582a9c2a5f37228dcd27283a74e1e Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Fri, 7 Jun 2024 17:48:34 +0900 Subject: [PATCH 1/4] fix a bug that perception_replayer/perception_reproducer can't publish old 'autoware_auto_perception_msgs' in old rosbags. Signed-off-by: temkei.kem <1041084556@qq.com> --- .../perception_replayer.py | 18 +++------ .../perception_replayer_common.py | 37 +++++++++++++++---- .../perception_reproducer.py | 18 +++------ 3 files changed, 39 insertions(+), 34 deletions(-) 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): From 291f8b66b80c7f0106b02241101374042b5c3c74 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 7 Jun 2024 08:54:27 +0000 Subject: [PATCH 2/4] style(pre-commit): autofix Signed-off-by: temkei.kem <1041084556@qq.com> --- .../perception_replayer/perception_replayer.py | 1 - .../perception_replayer_common.py | 13 +++++++++---- .../perception_replayer/perception_reproducer.py | 1 - 3 files changed, 9 insertions(+), 6 deletions(-) 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 c33b5dfe5..597c58a83 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -105,7 +105,6 @@ def on_timer(self): 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(): self.is_pause = True 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 9fa8890d0..425ded67d 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,9 +19,12 @@ from subprocess import check_output import time -from autoware_perception_msgs.msg import DetectedObjects, PredictedObjects, TrackedObjects -from autoware_perception_msgs.msg import TrafficLightGroupArray, TrafficLightGroup, TrafficLightElement - +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 from nav_msgs.msg import Odometry @@ -126,7 +129,9 @@ def load_rosbag(self, rosbag2_path: str): # 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. + 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: 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 1a914f28f..e4db8a92c 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -115,7 +115,6 @@ def on_timer(self): 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): From 68ff984562971417788b7bdf4e15e9741b3470c6 Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Mon, 10 Jun 2024 11:35:04 +0900 Subject: [PATCH 3/4] fix some spell errors. Signed-off-by: temkei.kem <1041084556@qq.com> --- .../perception_replayer_common.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 425ded67d..3f82c2c7d 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 @@ -141,17 +141,17 @@ def load_rosbag(self, rosbag2_path: str): # 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: + for traffic_signal in msg.signals: + traffic_light_group = TrafficLightGroup() + traffic_light_group.traffic_light_group_id = traffic_signal.traffic_signal_id + for traffic_signal_element in traffic_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) + traffic_light_group.elements.append(traffic_light_element) + new_msg.traffic_light_groups.append(traffic_light_group) msg = new_msg self.rosbag_traffic_signals_data.append((stamp, msg)) From 0b3fa1d0a4083a1d504bab2e90b74cc97503af83 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 10 Jun 2024 02:38:02 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- .../scripts/perception_replayer/perception_replayer_common.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 3f82c2c7d..85582a888 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 @@ -143,7 +143,9 @@ def load_rosbag(self, rosbag2_path: str): new_msg.stamp = msg.stamp for traffic_signal in msg.signals: traffic_light_group = TrafficLightGroup() - traffic_light_group.traffic_light_group_id = traffic_signal.traffic_signal_id + traffic_light_group.traffic_light_group_id = ( + traffic_signal.traffic_signal_id + ) for traffic_signal_element in traffic_signal.elements: traffic_light_element = TrafficLightElement() traffic_light_element.color = traffic_signal_element.color