@@ -54,7 +54,7 @@ def __init__(self, args):
54
54
self .perv_objects_msg , self .prev_traffic_signals_msg = self .find_topics_by_timestamp (
55
55
pose_timestamp
56
56
)
57
- self .memorized_unoised_objects_msg = (
57
+ self .memorized_original_objects_msg = (
58
58
self .memorized_noised_objects_msg
59
59
) = self .perv_objects_msg
60
60
@@ -101,8 +101,6 @@ def on_timer(self):
101
101
return
102
102
103
103
ego_pose = self .ego_odom .pose .pose
104
- # ego_speed = np.sqrt(self.ego_odom.twist.twist.linear.x ** 2 +
105
- # self.ego_odom.twist.twist.linear.y ** 2)
106
104
dist_moved = (
107
105
np .sqrt (
108
106
(ego_pose .position .x - self .last_sequenced_ego_pose .position .x ) ** 2
@@ -164,16 +162,18 @@ def on_timer(self):
164
162
165
163
# Add an additional constraint to avoid publishing too fast when there is a speed gap between the ego and the rosbag's ego when ego is departing/stopping while rosbag's ego is moving.
166
164
if not repeat_flag :
167
- ego_speed = np .sqrt (self .ego_odom .twist .twist .linear .x ** 2 +
168
- self .ego_odom .twist .twist .linear .y ** 2 )
165
+ ego_speed = np .sqrt (
166
+ self .ego_odom .twist .twist .linear .x ** 2 + self .ego_odom .twist .twist .linear .y ** 2
167
+ )
169
168
ego_odom_idx = self .reproduce_sequence_indices [0 ]
170
169
_ , ego_odom_msg = self .rosbag_ego_odom_data [ego_odom_idx ]
171
- ego_rosbag_speed = np .sqrt (ego_odom_msg .twist .twist .linear .x **
172
- 2 + ego_odom_msg .twist .twist .linear .y ** 2 )
173
-
170
+ ego_rosbag_speed = np .sqrt (
171
+ ego_odom_msg .twist .twist .linear .x ** 2 + ego_odom_msg .twist .twist .linear .y ** 2
172
+ )
173
+
174
174
ego_rosbag_dist = np .sqrt (
175
- (ego_pose .position .x - ego_odom_msg .pose .pose .position .x ) ** 2 +
176
- (ego_pose .position .y - ego_odom_msg .pose .pose .position .y ) ** 2
175
+ (ego_pose .position .x - ego_odom_msg .pose .pose .position .x ) ** 2
176
+ + (ego_pose .position .y - ego_odom_msg .pose .pose .position .y ) ** 2
177
177
)
178
178
repeat_flag = ego_rosbag_speed > ego_speed * 5 and ego_rosbag_dist > 1.0
179
179
# set the speed threshold to many (5) times then ego_speed because this constraint is mainly for departing/stopping (ego speed is close to 0).
@@ -259,7 +259,7 @@ def add_perception_noise(
259
259
if np .random .rand () < update_rate :
260
260
self .stopwatch .tic ("add noise" )
261
261
self .memorized_noised_objects_msg = self .copy_message (
262
- self .memorized_unoised_objects_msg
262
+ self .memorized_original_objects_msg
263
263
)
264
264
for obj in self .memorized_noised_objects_msg .objects :
265
265
noise_x = np .random .normal (0 , x_noise_std )
0 commit comments