Ask Your Question
0

tf transforms high latency

asked 2013-08-12 08:05:14 -0500

jy gravatar image

updated 2014-11-22 17:05:38 -0500

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Please provide more information for us to be able to help you better. What are you running? Do you get the error once or continuously? What is the actual error output? What are the code snippets you are running and what are the variations.

Tully gravatar imageTully ( 2013-08-12 08:12:02 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2013-08-12 08:38:00 -0500

tfoote gravatar image

Your problem is that you are constructing the listener inside the callback. This means that you do not give it time to build up it's buffer. That's why if you query it quickly, it reports it does not exist. And with a timeout, it builds up a buffer of new information, but it will never hear about the past where you queried it.

I recommend rereading the tf and time tutorial linked from the tutorial you linked to.

edit flag offensive delete link more

Comments

Thanks so much!

jy gravatar imagejy ( 2013-08-12 11:54:55 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-08-12 08:05:14 -0500

Seen: 593 times

Last updated: Aug 12 '13