2626from utils import create_empty_pointcloud
2727from utils import translate_objects_coordinate
2828
29- dist_eps = 1e-3 # (meters)
30-
3129
3230class PerceptionReproducer (PerceptionReplayerCommon ):
3331 def __init__ (self , args ):
34- self .rosbag_ego_odom_search_radius = (
35- args .search_radius
36- ) # (m) the range of the ego odom to search,
37- self .ego_odom_search_radius = (
38- self .rosbag_ego_odom_search_radius
39- ) # it may be set by an individual parameter.
40-
41- self .reproduce_cool_down = (
42- args .reproduce_cool_down if args .search_radius != 0.0 else 0.0
43- ) # (sec) the cool down time for republishing published data, please make sure that it's greater than the ego's stopping time.
32+ self .rosbag_ego_odom_search_radius = args .search_radius
33+ self .ego_odom_search_radius = self .rosbag_ego_odom_search_radius
34+ self .reproduce_cool_down = args .reproduce_cool_down if args .search_radius != 0.0 else 0.0
4435
4536 super ().__init__ (args , "perception_reproducer" )
4637
47- self .reproduce_sequence_indices = deque () # contains ego_odom_idx
48- self .cool_down_indices = deque () # contains ego_odom_idx
49- self .ego_odom_id2last_published_timestamp = {} # for checking last published timestamp.
50-
51- self .prev_ego_pos = None
52- self .prev_ego_odom_msg = None
53- self .perv_objects_msg = None
54- self .prev_traffic_signals_msg = None
55-
5638 self .stopwatch = StopWatch (self .args .verbose ) # for debug
5739
5840 # refresh cool down for setting initial pose in psim.
@@ -63,17 +45,27 @@ def __init__(self, args):
6345 # to make some data to accelerate computation
6446 self .preprocess_data ()
6547
48+ self .reproduce_sequence_indices = deque () # contains ego_odom_idx
49+ self .cool_down_indices = deque () # contains ego_odom_idx
50+ self .ego_odom_id2last_published_timestamp = {} # for checking last published timestamp.
51+ self .last_sequenced_ego_pose = None
52+
53+ pose_timestamp , self .prev_ego_odom_msg = self .rosbag_ego_odom_data [0 ]
54+ self .perv_objects_msg , self .prev_traffic_signals_msg = self .find_topics_by_timestamp (
55+ pose_timestamp
56+ )
57+ self .memorized_original_objects_msg = (
58+ self .memorized_noised_objects_msg
59+ ) = self .perv_objects_msg
60+
6661 # start main timer callback
67- time_diffs = []
68- prev_stamp = self .rosbag_ego_odom_data [0 ][0 ]
69- for stamp , _ in self .rosbag_ego_odom_data [1 :]:
70- time_diff = (stamp - prev_stamp ) / 1e9
71- time_diffs .append (time_diff )
72- prev_stamp = stamp
73-
74- average_ego_odom_interval = sum (time_diffs ) / len (time_diffs )
75- # slow down the publication speed.
76- average_ego_odom_interval *= args .publishing_speed_factor
62+
63+ average_ego_odom_interval = np .mean (
64+ [
65+ (self .rosbag_ego_odom_data [i ][0 ] - self .rosbag_ego_odom_data [i - 1 ][0 ]) / 1e9
66+ for i in range (1 , len (self .rosbag_ego_odom_data ))
67+ ]
68+ )
7769 self .timer = self .create_timer (average_ego_odom_interval , self .on_timer )
7870
7971 # kill perception process to avoid a conflict of the perception topics
@@ -105,30 +97,34 @@ def on_timer(self):
10597 pointcloud_msg = create_empty_pointcloud (timestamp_msg )
10698 self .pointcloud_pub .publish (pointcloud_msg )
10799
108- if not self .ego_pose :
109- print ("No ego pose found." )
100+ if not self .ego_odom :
101+ print ("No ego odom found." )
110102 return
111103
112- # Update reproduce list if ego_pos is moved.
113- if (
114- self .ego_pose is None
115- or self .prev_ego_pos is None
116- or (
117- (self .ego_pose .position .x - self .prev_ego_pos .position .x ) ** 2
118- + (self .ego_pose .position .y - self .prev_ego_pos .position .y ) ** 2
104+ ego_pose = self .ego_odom .pose .pose
105+ dist_moved = (
106+ np .sqrt (
107+ (ego_pose .position .x - self .last_sequenced_ego_pose .position .x ) ** 2
108+ + (ego_pose .position .y - self .last_sequenced_ego_pose .position .y ) ** 2
119109 )
120- > dist_eps ** 2
121- ):
110+ if self .last_sequenced_ego_pose
111+ else 999
112+ )
113+
114+ # Update the reproduce sequence if the distance moved is greater than the search radius.
115+ if dist_moved > self .ego_odom_search_radius :
116+ self .last_sequenced_ego_pose = ego_pose
117+
122118 # find the nearest ego odom by simulation observation
123119 self .stopwatch .tic ("find_nearest_ego_odom_by_observation" )
124120 nearby_ego_odom_indies = self .find_nearby_ego_odom_indies (
125- [self . ego_pose ], self .ego_odom_search_radius
121+ [ego_pose ], self .ego_odom_search_radius
126122 )
127123 nearby_ego_odom_indies = [
128124 self .rosbag_ego_odom_data [idx ][1 ].pose .pose for idx in nearby_ego_odom_indies
129125 ]
130126 if not nearby_ego_odom_indies :
131- nearest_ego_odom_ind = self .find_nearest_ego_odom_index (self . ego_pose )
127+ nearest_ego_odom_ind = self .find_nearest_ego_odom_index (ego_pose )
132128 nearby_ego_odom_indies += [
133129 self .rosbag_ego_odom_data [nearest_ego_odom_ind ][1 ].pose .pose
134130 ]
@@ -157,78 +153,80 @@ def on_timer(self):
157153 ]
158154 ego_odom_indices = sorted (ego_odom_indices )
159155 self .reproduce_sequence_indices = deque (ego_odom_indices )
160-
161156 self .stopwatch .toc ("update reproduce_sequence" )
162157
163- self .prev_ego_pos = self .ego_pose
158+ if self .args .verbose :
159+ print ("reproduce_sequence_indices: " , list (self .reproduce_sequence_indices )[:20 ])
160+
161+ # Get messages
162+ repeat_flag = len (self .reproduce_sequence_indices ) == 0
164163
165- # get data to publish
166- repeat_trigger = len (self .reproduce_sequence_indices ) == 0
167- if not repeat_trigger : # pop data from reproduce_sequence if sequence is not empty.
164+ # 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.
165+ if not repeat_flag :
166+ ego_speed = np .sqrt (
167+ self .ego_odom .twist .twist .linear .x ** 2 + self .ego_odom .twist .twist .linear .y ** 2
168+ )
169+ ego_odom_idx = self .reproduce_sequence_indices [0 ]
170+ _ , ego_odom_msg = self .rosbag_ego_odom_data [ego_odom_idx ]
171+ ego_rosbag_speed = np .sqrt (
172+ ego_odom_msg .twist .twist .linear .x ** 2 + ego_odom_msg .twist .twist .linear .y ** 2
173+ )
174+
175+ ego_rosbag_dist = np .sqrt (
176+ (ego_pose .position .x - ego_odom_msg .pose .pose .position .x ) ** 2
177+ + (ego_pose .position .y - ego_odom_msg .pose .pose .position .y ) ** 2
178+ )
179+ repeat_flag = ego_rosbag_speed > ego_speed * 5 and ego_rosbag_dist > 1.0
180+ # 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).
181+
182+ if not repeat_flag :
183+ self .stopwatch .tic ("find_topics_by_timestamp" )
168184 ego_odom_idx = self .reproduce_sequence_indices .popleft ()
185+ # extract messages by the nearest ego odom timestamp
169186 pose_timestamp , ego_odom_msg = self .rosbag_ego_odom_data [ego_odom_idx ]
170- # extract message by the nearest ego odom timestamp
171- self .stopwatch .tic ("find_topics_by_timestamp" )
172187 objects_msg , traffic_signals_msg = self .find_topics_by_timestamp (pose_timestamp )
173188 self .stopwatch .toc ("find_topics_by_timestamp" )
174- # if self.args.verbose:
175- # print("reproduce_sequence_indices: ", list( self.reproduce_sequence_indices)[:20])
176-
177- else : # get perv data to publish if reproduce_sequence is empty.
189+ # update cool down info.
190+ self .ego_odom_id2last_published_timestamp [ ego_odom_idx ] = timestamp
191+ self . cool_down_indices . append ( ego_odom_idx )
192+ else :
178193 ego_odom_msg = self .prev_ego_odom_msg
179194 objects_msg = self .perv_objects_msg
180195 traffic_signals_msg = self .prev_traffic_signals_msg
181196
182- # transform and publish current data .
197+ # Transform and publish messages .
183198 self .stopwatch .tic ("transform and publish" )
184-
185199 # ego odom
186- self .recorded_ego_pub .publish (ego_odom_msg )
187-
200+ if ego_odom_msg :
201+ self .prev_ego_odom_msg = ego_odom_msg
202+ self .recorded_ego_pub .publish (ego_odom_msg )
188203 # objects
204+ objects_msg = objects_msg if objects_msg else self .perv_objects_msg
189205 if objects_msg :
206+ self .perv_objects_msg = objects_msg
190207 objects_msg .header .stamp = timestamp_msg
208+
209+ # add noise to repeat published objects
210+ if repeat_flag and self .args .noise :
211+ objects_msg = self .add_perception_noise (objects_msg )
212+
191213 if self .args .detected_object :
192- # copy the messages
193- self .stopwatch .tic ("message deepcopy" )
194- objects_msg_copied = pickle .loads (
195- pickle .dumps (objects_msg )
196- ) # this is x5 faster than deepcopy
197- self .stopwatch .toc ("message deepcopy" )
198-
199- log_ego_pose = ego_odom_msg .pose .pose
200- translate_objects_coordinate (self .ego_pose , log_ego_pose , objects_msg_copied )
201- else :
202- objects_msg_copied = objects_msg
203- self .objects_pub .publish (objects_msg_copied )
214+ objects_msg = self .copy_message (objects_msg )
215+ translate_objects_coordinate (ego_pose , ego_odom_msg .pose .pose , objects_msg )
216+
217+ self .objects_pub .publish (objects_msg )
204218
205219 # traffic signals
220+ traffic_signals_msg = (
221+ traffic_signals_msg if traffic_signals_msg else self .prev_traffic_signals_msg
222+ )
206223 if traffic_signals_msg :
207224 traffic_signals_msg .stamp = timestamp_msg
208- self .traffic_signals_pub .publish (traffic_signals_msg )
209225 self .prev_traffic_signals_msg = traffic_signals_msg
210- elif self .prev_traffic_signals_msg :
211- self .prev_traffic_signals_msg .stamp = timestamp_msg
226+ self .traffic_signals_pub .publish (traffic_signals_msg )
212227
213- self .traffic_signals_pub .publish (self .prev_traffic_signals_msg )
214228 self .stopwatch .toc ("transform and publish" )
215229
216- if not repeat_trigger :
217- # save data for repeat publication.
218- self .prev_ego_odom_msg = ego_odom_msg
219- self .perv_objects_msg = (
220- objects_msg if objects_msg is not None else self .perv_objects_msg
221- )
222- self .prev_traffic_signals_msg = (
223- traffic_signals_msg
224- if traffic_signals_msg is not None
225- else self .prev_traffic_signals_msg
226- )
227-
228- # update cool down info.
229- self .ego_odom_id2last_published_timestamp [ego_odom_idx ] = timestamp
230- self .cool_down_indices .append (ego_odom_idx )
231-
232230 self .stopwatch .toc ("total on_timer" )
233231
234232 def find_nearest_ego_odom_index (self , ego_pose ):
@@ -247,10 +245,47 @@ def find_nearby_ego_odom_indies(self, ego_poses: list, search_radius: float):
247245
248246 return nearby_indices
249247
248+ def copy_message (self , msg ):
249+ self .stopwatch .tic ("message deepcopy" )
250+ objects_msg_copied = pickle .loads (pickle .dumps (msg )) # this is x5 faster than deepcopy
251+ self .stopwatch .toc ("message deepcopy" )
252+ return objects_msg_copied
253+
254+ def add_perception_noise (
255+ self , objects_msg , update_rate = 0.03 , x_noise_std = 0.1 , y_noise_std = 0.05
256+ ):
257+ if self .memorized_original_objects_msg != objects_msg :
258+ self .memorized_noised_objects_msg = self .memorized_original_objects_msg = objects_msg
259+
260+ if np .random .rand () < update_rate :
261+ self .stopwatch .tic ("add noise" )
262+ self .memorized_noised_objects_msg = self .copy_message (
263+ self .memorized_original_objects_msg
264+ )
265+ for obj in self .memorized_noised_objects_msg .objects :
266+ noise_x = np .random .normal (0 , x_noise_std )
267+ noise_y = np .random .normal (0 , y_noise_std )
268+ if self .args .detected_object or self .args .tracked_object :
269+ obj .kinematics .pose_with_covariance .pose .position .x += noise_x
270+ obj .kinematics .pose_with_covariance .pose .position .y += noise_y
271+ else :
272+ obj .kinematics .initial_pose_with_covariance .pose .position .x += noise_x
273+ obj .kinematics .initial_pose_with_covariance .pose .position .y += noise_y
274+ self .stopwatch .toc ("add noise" )
275+
276+ return self .memorized_noised_objects_msg
277+
250278
251279if __name__ == "__main__" :
252280 parser = argparse .ArgumentParser ()
253281 parser .add_argument ("-b" , "--bag" , help = "rosbag" , default = None )
282+ parser .add_argument (
283+ "-n" ,
284+ "--noise" ,
285+ help = "apply perception noise to the objects when publishing repeated messages" ,
286+ action = "store_true" ,
287+ default = True ,
288+ )
254289 parser .add_argument (
255290 "-d" , "--detected-object" , help = "publish detected object" , action = "store_true"
256291 )
@@ -273,16 +308,10 @@ def find_nearby_ego_odom_indies(self, ego_poses: list, search_radius: float):
273308 parser .add_argument (
274309 "-c" ,
275310 "--reproduce-cool-down" ,
276- help = "The cool down time for republishing published messages (default is 80.0 seconds)" ,
311+ help = "The cool down time for republishing published messages (default is 80.0 seconds), please make sure that it's greater than the ego's stopping time. " ,
277312 type = float ,
278313 default = 80.0 ,
279314 )
280- parser .add_argument (
281- "--publishing-speed-factor" ,
282- type = float ,
283- default = 1.2 ,
284- help = "A factor to slow down the publication speed." ,
285- )
286315
287316 args = parser .parse_args ()
288317
0 commit comments