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 6ebf29528..a8c85f7fb 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 @@ -37,7 +37,9 @@ def __init__(self, args): self.rosbag_ego_odom_search_radius = ( args.search_radius ) # (m) the range of the ego odom to search, - self.ego_odom_search_radius = self.rosbag_ego_odom_search_radius # it may be set by an individual parameter. + self.ego_odom_search_radius = ( + self.rosbag_ego_odom_search_radius + ) # it may be set by an individual parameter. self.reproduce_cool_down = ( args.reproduce_cool_down @@ -115,11 +117,17 @@ def on_timer(self): ): # find the nearest ego odom by simulation observation self.stopwatch.tic("find_nearest_ego_odom_by_observation") - nearby_ego_odom_indies = self.find_nearby_ego_odom_indies([self.ego_pose], self.ego_odom_search_radius) - nearby_ego_odom_indies = [self.rosbag_ego_odom_data[idx][1].pose.pose for idx in nearby_ego_odom_indies] + nearby_ego_odom_indies = self.find_nearby_ego_odom_indies( + [self.ego_pose], self.ego_odom_search_radius + ) + nearby_ego_odom_indies = [ + self.rosbag_ego_odom_data[idx][1].pose.pose for idx in nearby_ego_odom_indies + ] if not nearby_ego_odom_indies: nearest_ego_odom_ind = self.find_nearest_ego_odom_index(self.ego_pose) - nearby_ego_odom_indies += [self.rosbag_ego_odom_data[nearest_ego_odom_ind][1].pose.pose] + nearby_ego_odom_indies += [ + self.rosbag_ego_odom_data[nearest_ego_odom_ind][1].pose.pose + ] self.stopwatch.toc("find_nearest_ego_odom_by_observation") # find a list of ego odom around the nearest_ego_odom_pos. @@ -140,10 +148,12 @@ def on_timer(self): break self.stopwatch.tic("update reproduce_sequence") - ego_odom_indices = [idx for idx in ego_odom_indices if idx not in self.cool_down_indices] + ego_odom_indices = [ + idx for idx in ego_odom_indices if idx not in self.cool_down_indices + ] ego_odom_indices = sorted(ego_odom_indices) self.reproduce_sequence_indices = deque(ego_odom_indices) - + self.stopwatch.toc("update reproduce_sequence") self.prev_ego_pos = self.ego_pose @@ -157,8 +167,8 @@ def on_timer(self): self.stopwatch.tic("find_topics_by_timestamp") objects_msg, traffic_signals_msg = self.find_topics_by_timestamp(pose_timestamp) self.stopwatch.toc("find_topics_by_timestamp") - if True:#self.args.verbose: - print('reproduce_sequence_indices: ',list(self.reproduce_sequence_indices)[:30]) + if True: # self.args.verbose: + print("reproduce_sequence_indices: ", list(self.reproduce_sequence_indices)[:30]) else: # get perv data to publish if reproduce_sequence is empty. ego_odom_msg = self.prev_ego_odom_msg @@ -231,7 +241,9 @@ def find_nearby_ego_odom_indies(self, ego_poses, search_radius: float): search_radius: the radius to search around the ego_poses. """ ego_poses_np = np.array([[pose.position.x, pose.position.y] for pose in ego_poses]) - dists_squared = np.sum((self.rosbag_ego_odom_data_numpy[:, None] - ego_poses_np) ** 2, axis=2) + dists_squared = np.sum( + (self.rosbag_ego_odom_data_numpy[:, None] - ego_poses_np) ** 2, axis=2 + ) nearby_indices = np.where(np.any(dists_squared <= search_radius**2, axis=1))[0] return nearby_indices