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 c33b5dfe..597c58a8 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 ea594613..22371440 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 @@ -31,7 +34,8 @@ from rosbag2_py import StorageFilter from rosidl_runtime_py.utilities import get_message from sensor_msgs.msg import PointCloud2 -from utils import open_reader,get_starting_time +from utils import get_starting_time +from utils import open_reader class PerceptionReplayerCommon(Node): @@ -85,9 +89,13 @@ def __init__(self, args, name): # load rosbag print("Stared loading rosbag") if os.path.isdir(args.bag): - bags = [ os.path.join(args.bag, base_name) for base_name in os.listdir(args.bag) if base_name.endswith(args.rosbag_format) ] + 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) + self.load_rosbag(bag_file) else: self.load_rosbag(args.bag) print("Ended loading rosbag") @@ -126,7 +134,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 1a914f28..e4db8a92 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): 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 b8196156..e64368d9 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 @@ -15,8 +15,8 @@ # limitations under the License. import argparse -import pickle from collections import deque +import pickle import numpy as np from perception_replayer_common import PerceptionReplayerCommon @@ -25,28 +25,33 @@ from utils import create_empty_pointcloud from utils import translate_objects_coordinate -dist_eps = 1e-2 # (meters) +dist_eps = 1e-2 # (meters) + class PerceptionReproducerV2(PerceptionReplayerCommon): - ''' + """ PerceptionReproducerV2 is used reproduces perception topic data from rosbag, specifically optimized for use with simulators. - ''' + """ + def __init__(self, args): - self.rosbag_ego_odom_seach_radius = args.seach_radius # (m) the range of the ego odom to search, - self.reproduce_cooldown = args.reproduce_cooldown # (sec) the cooldown time for republishing published data, please make sure that it's greater than the ego's stopping time. - + self.rosbag_ego_odom_seach_radius = ( + args.seach_radius + ) # (m) the range of the ego odom to search, + self.reproduce_cooldown = ( + args.reproduce_cooldown + ) # (sec) the cooldown time for republishing published data, please make sure that it's greater than the ego's stopping time. + super().__init__(args, "perception_reproducer_v2") - - self.reproduce_sequence_indices = deque()# contains ego_odom_idx - self.cooldown_indices = deque() # contains ego_odom_idx - self.ego_odom_id2last_published_timestamp=dict() # for checking last published timestap. + + self.reproduce_sequence_indices = deque() # contains ego_odom_idx + self.cooldown_indices = deque() # contains ego_odom_idx + self.ego_odom_id2last_published_timestamp = dict() # for checking last published timestap. self.prev_ego_pos = None self.prev_ego_odom_msg = None self.perv_objects_msg = None self.prev_traffic_signals_msg = None - self.stopwatch = StopWatch(self.args.verbose) # for debug # to make some data to accelerate computation @@ -56,10 +61,10 @@ def __init__(self, args): time_diffs = [] prev_stamp = self.rosbag_ego_odom_data[0][0] for stamp, _ in self.rosbag_ego_odom_data[1:]: - time_diff = (stamp - prev_stamp)/ 1e9 + time_diff = (stamp - prev_stamp) / 1e9 time_diffs.append(time_diff) prev_stamp = stamp - + average_ego_odom_interval = sum(time_diffs) / len(time_diffs) self.timer = self.create_timer(average_ego_odom_interval, self.on_timer) @@ -95,45 +100,49 @@ def on_timer(self): if not self.ego_pose: print("No ego pose found.") return - - ## Update reproduce list if ego_pos is moved. - if self.ego_pose is None \ - or self.prev_ego_pos is None or \ - ((self.ego_pose.position.x - self.prev_ego_pos.position.x)**2 + \ - (self.ego_pose.position.y - self.prev_ego_pos.position.y)**2) > dist_eps**2: + ## Update reproduce list if ego_pos is moved. + if ( + self.ego_pose is None + or self.prev_ego_pos is None + or ( + (self.ego_pose.position.x - self.prev_ego_pos.position.x) ** 2 + + (self.ego_pose.position.y - self.prev_ego_pos.position.y) ** 2 + ) + > dist_eps**2 + ): # find the nearest ego odom by simulation observation self.stopwatch.tic("find_nearest_ego_odom_by_observation") nearst_ego_odom_ind = self.find_nearest_ego_odom_index(self.ego_pose) nearst_ego_odom_pos = self.rosbag_ego_odom_data[nearst_ego_odom_ind][1].pose.pose self.stopwatch.toc("find_nearest_ego_odom_by_observation") - # find a list of ego odoms around the nearst_ego_odom_pos. + # find a list of ego odoms around the nearst_ego_odom_pos. self.stopwatch.tic("find_nearby_ego_odom_indies") - ego_odom_indices = self.find_nearby_ego_odom_indies(nearst_ego_odom_pos, - self.rosbag_ego_odom_seach_radius) + ego_odom_indices = self.find_nearby_ego_odom_indies( + nearst_ego_odom_pos, self.rosbag_ego_odom_seach_radius + ) self.stopwatch.toc("find_nearby_ego_odom_indies") # update reproduce_sequence with those data not in cooldown list. while self.cooldown_indices: last_timestamp = self.ego_odom_id2last_published_timestamp[self.cooldown_indices[0]] - if (timestamp - last_timestamp).nanoseconds/1e9 > self.reproduce_cooldown: + if (timestamp - last_timestamp).nanoseconds / 1e9 > self.reproduce_cooldown: self.cooldown_indices.popleft() else: break - self.stopwatch.tic("update reproduce_sequence") self.reproduce_sequence_indices = deque( - [idx for idx in ego_odom_indices - if idx not in self.cooldown_indices]) + [idx for idx in ego_odom_indices if idx not in self.cooldown_indices] + ) self.stopwatch.toc("update reproduce_sequence") self.prev_ego_pos = self.ego_pose - + ## get data to publish repeat_trigger = len(self.reproduce_sequence_indices) == 0 - if not repeat_trigger:# pop data from reproduce_sequence if sequence is not empty. + if not repeat_trigger: # pop data from reproduce_sequence if sequence is not empty. ego_odom_idx = self.reproduce_sequence_indices.popleft() pose_timestamp, ego_odom_msg = self.rosbag_ego_odom_data[ego_odom_idx] # extract message by the nearest ego odom timestamp @@ -141,7 +150,7 @@ def on_timer(self): objects_msg, traffic_signals_msg = self.find_topics_by_timestamp(pose_timestamp) self.stopwatch.toc("find_topics_by_timestamp") - else:#get perv data to publish if reproduce_sequence is empty. + else: # get perv data to publish if reproduce_sequence is empty. ego_odom_msg = self.prev_ego_odom_msg objects_msg = self.perv_objects_msg traffic_signals_msg = self.prev_traffic_signals_msg @@ -158,7 +167,9 @@ def on_timer(self): if self.args.detected_object: # copy the messages self.stopwatch.tic("message deepcopy") - objects_msg_copied = pickle.loads(pickle.dumps(objects_msg)) # this is x5 faster than deepcopy + objects_msg_copied = pickle.loads( + pickle.dumps(objects_msg) + ) # this is x5 faster than deepcopy self.stopwatch.toc("message deepcopy") log_ego_pose = ego_odom_msg.pose.pose @@ -167,7 +178,6 @@ def on_timer(self): objects_msg_copied = objects_msg self.objects_pub.publish(objects_msg_copied) - # traffic signals if traffic_signals_msg: traffic_signals_msg.stamp = timestamp_msg @@ -181,8 +191,14 @@ def on_timer(self): if not repeat_trigger: # save data for repeate pubulication. self.prev_ego_odom_msg = ego_odom_msg - self.perv_objects_msg = objects_msg if objects_msg is not None else self.perv_objects_msg - self.prev_traffic_signals_msg = traffic_signals_msg if traffic_signals_msg is not None else self.prev_traffic_signals_msg + self.perv_objects_msg = ( + objects_msg if objects_msg is not None else self.perv_objects_msg + ) + self.prev_traffic_signals_msg = ( + traffic_signals_msg + if traffic_signals_msg is not None + else self.prev_traffic_signals_msg + ) # update cooldown info. self.ego_odom_id2last_published_timestamp[ego_odom_idx] = timestamp @@ -196,18 +212,20 @@ def find_nearest_ego_odom_index(self, ego_pose): dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1) nearest_idx = np.argmin(dists_squared) return nearest_idx - + def find_nearby_ego_odom_indies(self, ego_pose, search_radius: float): - ''' + """ Find the indices of the ego odoms that are within a certain distance of the ego_pose. Inputs: ego_pose: the ego pose to search around. - search_radius: the radius to search around the ego_pose.''' + search_radius: the radius to search around the ego_pose.""" self_pose = np.array([ego_pose.position.x, ego_pose.position.y]) dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1) nearby_indices = np.where(dists_squared <= search_radius**2)[0] return nearby_indices + + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("-b", "--bag", help="rosbag", default=None) @@ -224,10 +242,18 @@ def find_nearby_ego_odom_indies(self, ego_pose, search_radius: float): "-v", "--verbose", help="output debug data", action="store_true", default=False ) parser.add_argument( - "-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 + "-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=40.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() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index 48cc79e8..34489f5b 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -25,10 +25,12 @@ from tf_transformations import euler_from_quaternion from tf_transformations import quaternion_from_euler + def get_starting_time(uri: str): info = rosbag2_py.Info().read_metadata(uri, "sqlite3") return info.starting_time + def get_rosbag_options(path, serialization_format="cdr"): storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3")