From 53ad0e6456a11381338c9773c95a70dbdc3081fa Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Fri, 7 Jun 2024 16:00:04 +0900 Subject: [PATCH] fix: convert old autoware_auto_perception_msgs in rosbag to new autoware_perception_msgs; feat: new reception_reproducer. --- planning/planning_debug_tools/CMakeLists.txt | 1 + .../perception_replayer.py | 17 ++------ .../perception_replayer_common.py | 40 +++++++++++++++---- .../perception_reproducer.py | 17 ++------ .../perception_reproducer_v2.py | 22 ++++------ 5 files changed, 48 insertions(+), 49 deletions(-) diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index 389bfdaa..0fd805f4 100644 --- a/planning/planning_debug_tools/CMakeLists.txt +++ b/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} 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 5f9e9575..597c58a8 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,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(): 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 2cf54a4f..b8cbc20c 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,35 @@ 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 b2b6a3c0..e4db8a92 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/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") diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer_v2.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer_v2.py index cc56046b..8d2b8497 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer_v2.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer_v2.py @@ -53,7 +53,7 @@ def __init__(self, args): self.preprocess_data() # start main timer callback - self.timer = self.create_timer(0.1, self.on_timer) + self.timer = self.create_timer(0.05, self.on_timer) # kill perception process to avoid a conflict of the perception topics self.timer_check_perception_process = self.create_timer(3.0, self.on_timer_kill_perception) @@ -162,20 +162,12 @@ def on_timer(self): # traffic signals if traffic_signals_msg: - if self.is_auto_traffic_signals:# temporary support old auto msgs - traffic_signals_msg.header.stamp = timestamp_msg - self.auto_traffic_signals_pub.publish(traffic_signals_msg) - else: - traffic_signals_msg.stamp = timestamp_msg - self.traffic_signals_pub.publish(traffic_signals_msg) - + traffic_signals_msg.stamp = timestamp_msg + 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:# temporary support old auto msgs - self.prev_traffic_signals_msg.header.stamp = timestamp_msg - self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) - else: - self.prev_traffic_signals_msg.stamp = timestamp_msg - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + self.prev_traffic_signals_msg.stamp = timestamp_msg + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) self.stopwatch.toc("transform and publish") if not repeat_trigger: @@ -227,7 +219,7 @@ def find_nearby_ego_odom_indies(self, ego_pose, search_radius: float): "-r", "--seach-radius", help="the search radius for searching rosbag's ego odom messages around the nearest ego odom pose (default is 1 meters)", type=float, default=1.0 ) parser.add_argument( - "-c", "--reproduce-cooldown", help="The cooldown time for republishing published messages (default is 40.0 seconds)", type=float, default=30.0 + "-c", "--reproduce-cooldown", help="The cooldown time for republishing published messages (default is 40.0 seconds)", type=float, default=40.0 ) args = parser.parse_args()