Tf python stuck in wait for transform extrapolation into past/future
Hi
I am currently publishing msgs out of a bag file, and naturally the time will go backwards everytime I re-run my bagfile. I want my nodes to gracefully restart like RVIZ
Here is my code snippet - inside callback function
if self.tmap_loaded == True and self.tmap_prop_loaded == True:
# detection of negative time step
dt = (rospy.Time.now() - self.last_update_ros_time)
self.last_update_ros_time = rospy.Time.now()
if dt.to_sec() < 0:
print "Negative timestep Clearing tf buffer"
self.tf_L.clear()
time.sleep(10)
print "Cleared Buffer"
return
# generate a pointcloud to represent the UWB FOV mask
uwb_mask_pcd = self._gen_pointcloud_uwb_fov(data)
exception = False
try:
# transform points in mask using current tf transform
self.tf_L.waitForTransform("/map","/body",uwb_mask_pcd.header.stamp,rospy.Dur.(1.0))
uwb_mask_pcd = self.tf_listener.transformPointCloud("/map", uwb_mask_pcd)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# print str(e)
exception = True
if exception == False:
self._process_uwb_scan(data, uwb_mask_pcd)
Here is the output of the code after the bagfiles stops transmitting (clock stops)
[ERROR] [WallTime: 1396791647.721125] [1354679082.276449] bad callback: <bound method self.tf_listener.waitForTransform("/map",
"/body", uwb_mask_pcd.header.stamp, rospy.Duration(1.0)) Exception: Lookup would require extrapolation into the past. Requested time 1354679074.849999905 but the earliest data is at time 1354679075.295000076, when looking up transform from frame [/body] to frame [/map]
[ERROR] [WallTime: 1396791648.735426] [1354679083.286904] bad callback: <bound method self.tf_listener.waitForTransform("/map",
"/body", uwb_mask_pcd.header.stamp, rospy.Duration(1.0)) Exception: Lookup would require extrapolation into the past. Requested time 1354679074.944000006 but the earliest data is at time 1354679075.295000076, when looking up transform from frame [/body] to frame [/map]
[ERROR] [WallTime: 1396791649.755686] [1354679084.309238] bad callback: <bound method self.tf_listener.waitForTransform("/map",
"/body", uwb_mask_pcd.header.stamp, rospy.Duration(1.0)) Exception: Lookup would require extrapolation into the past. Requested time 1354679075.038000107 but the earliest data is at time 1354679075.295000076, when looking up transform from frame [/body] to frame [/map]
So the bagfile stops publishing clock, yet waitForTransform looks like its stuck, because the error msgs keep popping up. Then I Restart the BagFile
This is the prints I get
Negative timestep Clearing tf buffer then a single unable to look up transforms cache empty and then a bunch of extrapolation into the past errors for waitForTransform. Once print "Cleared Buffer" Occurs I get a bunch of extrapolation into Past and Then Future.... it keeps alternating between them.
I thought waitForTransform is meant to wait only 1 second, but what I think happened is it uses the clock to tell when one second elapsed. I think its trying to wait until the last_time in bag file + 1 second. This time will never come, since I am using sim_time so the clock is controlled by the bag file. Therefore even when I restart the bag file the code is stuck in waitForTransform.
I am using the waitForTransform function to avoid small extrapolations into the future. Is there any way I can fix this? Or Force waitForTransform to exit?