ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Correct way to publish a 3D PointStamped target for head tracking?

asked 2011-08-19 10:59:16 -0600

Pi Robot gravatar image

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

edit retag flag offensive close merge delete

Comments

Just wondering, is this running across multiple computers or do you see the same error if it's all on one machine?
Eric Perko gravatar image Eric Perko  ( 2011-08-19 11:08:48 -0600 )edit
Everything is running on one machine and I have tried the code on two different computers. I should have mentioned that I am running Ubuntu 10.04 with Electric debian packages. (One machine was using the last pre-RC-1 Electric and I just now tested with RC-1).
Pi Robot gravatar image Pi Robot  ( 2011-08-19 11:19:41 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2011-08-19 12:16:12 -0600

dornhege gravatar image

updated 2011-08-19 12:16:33 -0600

I think what's happening, when you don't set the timestamp, is that the timestamp is 0. If you give an explicit time of 0 tf will transform using the latest transform available, the same probably works when it's the timestamp of the header. So this is actually a usable solution.

Although it might not make a big difference, depending on what you are willing to achieve it might be a bit more correct to set the timestamp when you compute the point as you tried. In that case you need to waitForTransform (not sure if there is a python equivalent) until the tf for the timestamp is available.

edit flag offensive delete link more

Comments

Thanks Christian--I think you nailed it. Adding the following line before the call to transformPoint in the code listing above fixes the problem: self.tf.waitForTransform(self.camera_link, target.header.frame_id, rospy.Time.now(), rospy.Duration(1.0)). So now I can set the timestamp to rospy.Time.now() on the target point and the head tracker node no longer complains with the ExtrapolationException error.
Pi Robot gravatar image Pi Robot  ( 2011-08-19 12:51:27 -0600 )edit
For the waitForTransform call it would be sufficient to set the time to target.header.stamp instead of now().
dornhege gravatar image dornhege  ( 2011-08-20 08:10:41 -0600 )edit
Ah, excellent!
Pi Robot gravatar image Pi Robot  ( 2011-08-20 10:14:10 -0600 )edit

Question Tools

Stats

Asked: 2011-08-19 10:59:16 -0600

Seen: 963 times

Last updated: Aug 19 '11