Pointing PR2 head at something in the camera frame

Question

I'm running a face detector, and would like to be able to point the head at a detected face. As it stands now, I get the face pixel coordinates, use the camera geometry to project that point to a 3D ray, and add a frame with that ray as a child of the optical frame of the camera I'm using. This all works fine - the problem comes when I point the head at that new frame. Since it's a child of the optical frame, when the head moves, the face frame moves along with it making it so once the move head action is done, the face frame is in a different location, so the head moves again.

I've tried adding the face frame as a child of some frame that doesn't move with the head (base_link), but am having trouble getting this to work right since to lookup that transform, I have to broadcast the face frame first, then lookup base_link -> face not knowing if the head is currently moving.

This seems like a common task that should have a standard solution. What's the right way to point the head at something detected in a camera image?

---

Originally posted by Dan Lazewatsky on ROS Answers with karma: 9115 on 2011-07-28

Post score: 0

Answer 1

I would use TF's transformPose to get a pose in some fixed frame and use that pose to point the head.

Create a new tf::Stamped<tf::Pose> with the frame id of your face frame and the current time as stamp and make it an identity pose, i.e. with a vector of (0 0 0) and a quaternion of (0 0 0 1). Then use tf::TransformListener::transformPose to transform that pose into some fixed frame, e.g. base_footprint (if the robot doesn't move) or map. Then use the resulting pose for pointing the head.

---

Originally posted by Lorenz with karma: 22731 on 2011-07-28

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Dan Lazewatsky on 2011-07-28:
That did it! I actually used transformPoint instead since I don't care about orientation.