From 54cc29fcaf3a5dbb9bddf1b8ea14fda6aae2d4c7 Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Mon, 10 Jun 2024 14:48:25 +0900 Subject: [PATCH] update default cool down time. --- .../perception_reproducer_v2.py | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) 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 5da192c1..57c4df71 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,21 +37,21 @@ def __init__(self, args): self.rosbag_ego_odom_search_radius = ( args.search_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.reproduce_cool_down = ( + args.reproduce_cool_down + ) # (sec) the cool down time for republishing published data, please make sure that it's greater than the ego's stopping time. self.rosbag_ego_odom_search_radius = ( args.search_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.reproduce_cool_down = ( + args.reproduce_cool_down + ) # (sec) the cool down 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.cool_down_indices = deque() # contains ego_odom_idx self.ego_odom_id2last_published_timestamp = dict() # for checking last published timestamp. self.prev_ego_pos = None @@ -134,17 +134,17 @@ def on_timer(self): ) 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: - self.cooldown_indices.popleft() + # update reproduce_sequence with those data not in cool down list. + while self.cool_down_indices: + last_timestamp = self.ego_odom_id2last_published_timestamp[self.cool_down_indices[0]] + if (timestamp - last_timestamp).nanoseconds / 1e9 > self.reproduce_cool_down: + self.cool_down_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.cool_down_indices] ) self.stopwatch.toc("update reproduce_sequence") @@ -210,9 +210,9 @@ def on_timer(self): else self.prev_traffic_signals_msg ) - # update cooldown info. + # update cool down info. self.ego_odom_id2last_published_timestamp[ego_odom_idx] = timestamp - self.cooldown_indices.append(ego_odom_idx) + self.cool_down_indices.append(ego_odom_idx) self.stopwatch.toc("total on_timer") @@ -260,10 +260,10 @@ def find_nearby_ego_odom_indies(self, ego_pose, search_radius: float): ) parser.add_argument( "-c", - "--reproduce-cooldown", - help="The cooldown time for republishing published messages (default is 40.0 seconds)", + "--reproduce-cool-down", + help="The cool down time for republishing published messages (default is 15.0 seconds)", type=float, - default=40.0, + default=15.0, ) args = parser.parse_args()