tf transforms high latency

edit

I'm experiencing really high latency looking up transforms.

Using ROS Groovy on the PR2

Was going through the tutorial at www ros org/wiki/image_geometry/Tutorials/ProjectTfFrameToImage and using it to modify my own code.

Problems around line 52 and 54 of "the code" when trying to lookup the transform between /r_wrist_roll_link and /wide_stereo_optical_frame.

With the default timeout and acquisition time, I get only the exception "Frame id /wide_stereo_optical_frame does not exist! Frames (1):"

In order to get it to work at all, I have to set the timeout very high, around a second, and also ask for the most recent transform with ros::Time(0) instead of the camera acquisition time.

rosrun tf tf_monitor shows that /r_wrist_roll_link and /wide_stereo_optical_frame are both publishing at 50Hz, so it is unclear why looking up a (any) transform is taking so long.

The plain tutorial code works fine, it's just when I try to do the same thing in my own project that problems happen. Clearly something is wrong, but I have made as few changes as possible.

[EDIT]

Here are the errors

[ WARN] [1376332000.619905274]: [imageCallback] TF exception:
  Lookup would require extrapolation into the past.  Requested time 1376332000.588570505 but the earliest data is at time 1376332000.612975642, when looking up transform from frame [/r_wrist_roll_link] to frame [/wide_stereo_optical_frame]

And originally I got (without increasing timeout)

[ WARN] [1376330015.046842618]: [imageCallback] TF exception:
  Frame id /wide_stereo_optical_frame does not exist! Frames (1):

Here is my code.

#include <opencv2/opencv.hpp>
#include <vector>

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <opencv_test/show_imageConfig.h>

#include "target.h"

namespace enc = sensor_msgs::image_encodings;

const char *WINDOW_NAME = "OpenCV ROS Demo";
cv::Mat frame;
Target target;

void drCallback(opencv_test::show_imageConfig &config, uint32_t level) {
    target.setBinaryThresh(config.binaryThresh);
}

void drawMarkers(std::vector<Marker> markers, cv::Mat &output) {
    cv::Mat_<cv::Vec3b> output_ = output;
    // Show them
    //drawClusters(horizMatches, output_, Scalar(0,255,0));
    //drawClusters(vertMatches, output_, Scalar(255,0,255));
    //circle(output_, isect.pos, 10, Scalar(0,0,255));
    for (const Marker &m : markers) {
        cv::circle(output_, m.pos, m.h_len, cv::Scalar(255,255,0));
    }
}

image_geometry::PinholeCameraModel cam_model;
std::string frameId = "/r_wrist_roll_link";
//std::string frameId = "/high_def_optical_frame";

void imageCallback (const sensor_msgs::Image::ConstPtr &srcImg,
                    const sensor_msgs::CameraInfo::ConstPtr &srcInfo) {
    cv_bridge::CvImagePtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvCopy(srcImg, enc::BGR8);
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    frame = cv_ptr->image;
    std::vector<Marker> markers = target.processFrame(frame);
    drawMarkers(markers, frame);

    cam_model.fromCameraInfo(srcInfo);
    tf::TransformListener tf_listener;
    tf::StampedTransform transform;
    ros::Time acq_time = srcInfo->header.stamp;
    ros::Duration timeout(20.0/30);
    try {
        tf_listener.waitForTransform(cam_model.tfFrame(), frameId,
                                     ros::Time(0), timeout);
        tf_listener.lookupTransform(cam_model.tfFrame(), frameId, ros::Time(0), transform);\

        // == Doesn't work ==
        // tf_listener.waitForTransform(cam_model.tfFrame(), frameId,
        //                              acq_time, timeout);
        // tf_listener.lookupTransform(cam_model.tfFrame(), frameId, acq_time, transform);
    } catch (tf::TransformException &ex) {
        ROS_WARN("[imageCallback] TF exception:\n  %s", ex ...