waitForTransform gets slower and slower

edit

I'm running ros hydro on ubuntu 12.04.

I'm using openni_tracker v0.2.0. I'm trying to write a python script that draws an arrow between the user's head and right hand, using frames published by the skeleton tracker.

The two transforms kinect_link -> head_1 and kinect_link -> right_hand_1 are both getting published consistently at a rate of 30 hz.

Here are two snippets of my code:

  1. tf_listener.waitForTransform('head_1', 'right_hand_1',  rospy.Time(0), rospy.Duration(0.5))
  ...
  2. hand_pose_hand_frame = PoseStamped()
  3. hand_pose_hand_frame.header.frame_id = 'right_hand_1'
  4. hand_pose_hand_frame.pose.orientation = Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)
  5. hand_pose = tf_listener.transformPose('head_1', hand_pose_hand_frame)

I need line 5 because I'm drawing the arrow in the head_1 frame. I've found that if I don't include line 1, then line 5 will often fail because the transform doesn't exist. However, line 1 is problematic. The longer my code runs, the slower line 1 runs, until eventually every call times out. I used python's time.clock() to profile my code's performance over time:

Since the necessary transform information is getting published at 30 hz the whole time, I can't figure out what waitForTransform is actually waiting for.

How can I fix my call to waitForTransform so that it has the same performance at T=25 as it does at T=5?