Ask Your Question
0

Tf python stuck in wait for transform extrapolation into past/future

asked 2014-04-06 04:00:28 -0500

Sentinal_Bias gravatar image

updated 2016-10-24 09:00:38 -0500

ngrennan gravatar image

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?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2014-04-07 15:52:39 -0500

tfoote gravatar image

When the clock stops being published time stops. Including for the timeout, so it may never timeout. Also make sure that you have a separate thread servicing other callbacks for the tf data. If you are blocking the only recieve callback queue, you will not receive the updates which will let it continue.

If this is part of a pipeline it is much better to use a tf::MessageFilter instead of waiting.

edit flag offensive delete link more

Comments

Thanks I wanted to use tf::MessageFilters... but I have written my node in python. There doesn't appear to be a tf msg Filter in the python tf api... http://answers.ros.org/question/49069/is-there-a-tfmessagefilter-in-the-tf-python-api/

Sentinal_Bias gravatar image Sentinal_Bias  ( 2014-04-08 16:49:50 -0500 )edit
0

answered 2014-04-06 15:31:41 -0500

Sentinal_Bias gravatar image

updated 2014-04-06 16:42:10 -0500

trans_flag = self.tf_listener.canTransform("/map", "/body", uwb_mask_pcd.header.stamp)
      while (trans_flag == False and timeout_counter < 30 ):
             trans_flag = self.tf_listener.canTransform("/map", "/body", uwb_mask_pcd.header.stamp)
             timeout_counter = timeout_counter + 1 
             time.sleep(0.1)

This is the code i used to simulate the tf.waitForTransform Function

Strangely i noticed when my bag file stops playing, the callbackfunction, which is subscribed to a topic publish from the bagfile keeps getting called back!! AND this is the source of the error, it looked like the code was stuck in the waitForTransform because the clock would not move when the bagfile stops playing.

But I think is may be msgs that are in the msg que...

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-04-06 04:00:28 -0500

Seen: 1,200 times

Last updated: Apr 07 '14