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 da97f918..61aa5ab5 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 @@ -142,7 +142,7 @@ def load_rosbag(self, rosbag2_path: str): 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. + ) # it's unsafe because the elements inside the message are still the old type, but it works for now on because they share the same elements' names and structures. 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 b2872691..9f733ee2 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -18,6 +18,7 @@ from collections import deque import pickle +from geometry_msgs.msg import PoseWithCovarianceStamped import numpy as np from perception_replayer_common import PerceptionReplayerCommon import rclpy @@ -25,7 +26,7 @@ from utils import create_empty_pointcloud from utils import translate_objects_coordinate -dist_eps = 1e-2 # (meters) +dist_eps = 1e-3 # (meters) class PerceptionReproducer(PerceptionReplayerCommon): @@ -54,6 +55,11 @@ def __init__(self, args): self.stopwatch = StopWatch(self.args.verbose) # for debug + # refresh cool down for setting initial pose in psim. + self.sub_init_pos = self.create_subscription( + PoseWithCovarianceStamped, "/initialpose", lambda msg: self.cool_down_indices.clear(), 1 + ) + # to make some data to accelerate computation self.preprocess_data() @@ -66,6 +72,8 @@ def __init__(self, args): prev_stamp = stamp average_ego_odom_interval = sum(time_diffs) / len(time_diffs) + # slow down the publication speed. + average_ego_odom_interval *= args.publishing_speed_factor self.timer = self.create_timer(average_ego_odom_interval, self.on_timer) # kill perception process to avoid a conflict of the perception topics @@ -269,6 +277,13 @@ def find_nearby_ego_odom_indies(self, ego_poses: list, search_radius: float): type=float, default=80.0, ) + parser.add_argument( + "--publishing-speed-factor", + type=float, + default=1.2, + help="A factor to slow down the publication speed.", + ) + args = parser.parse_args() rclpy.init()