Correct way to publish a 3D PointStamped target for head tracking?
Hello ROS fans,
I am playing around with head tracking where the target can be any 3D PointStamped target in any frame that is part of the robot's URDF model. The target point is published at 20Hz on a topic called /target_point that is subscribed to by the head tracking node that sends commands to the pan and tilt servos to keep the target centered in the camera view (a Kinect as it turns out.)
However, when publishing the PointStamped target, I am confused about what I should set the header.stamp value to be. If I set the stamp to be rospy.Time.now(), then my head tracking node complains with errors of the form:
ExtrapolationException: Lookup would require extrapolation into the future. Requested time 1313792150.750056028 but the latest data is at time 1313792150.706260920, when looking up transform from frame [/head_pan_link] to frame [/kinect_link]
On the other hand, if I don't set the target point's stamp at all, tracking actually works fine but I'm not sure how it can without the timestamp.
The relevant code in the head tracking node is this first part of the callback for the /target_point subscriber:
self.camera_link = '/kinect_link'
def update_head_position(self, target):
# Project the target point onto the camera link
camera_target = self.tf.transformPoint(self.camera_link, target)
# The virtual camera image is in the y-z plane.
pan = -camera_target.point.y
tilt = -camera_target.point.z
where "/kinect_link" is part of the URDF model of the robot and the ExtrapolationException error occurs at the call to tf.transformPoint.
So what am I missing that would allow the publishing of a timestamp with the target point while getting rid of the ExtrapolationException errors?
Thanks!
patrick