search

cra-ros-pkg / robot_localization

robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. Please ask questions on answers.ros.org.
http://www.cra.com
Other
1.38k stars 888 forks source link

Future transform issue #25

Closed mikepurvis closed 10 years ago

mikepurvis commented 10 years ago

I'm getting the same issue as this guy: http://answers.ros.org/question/188046/tf-lookup-would-require-extrapolation-into-the-future/

Node: /ekf_localization
Time: 16:16:48.307097616 (2014-09-02)
Severity: Warn
Published Topics: /odometry/filtered, /rosout, /tf

Could not obtain transform from odom to base_link. Error was Lookup would require extrapolation into the future.  Requested time 1409689008.298073576 but the latest data is at time 1409689008.286969633, when looking up transform from frame [odom] to frame [base_link]

Location:
/tmp/buildd/ros-indigo-robot-localization-2.1.3-0trusty-20140805-0104/include/robot_localization/ros_filter.h:RosFilter<Filter>::preparePose:1049

It's particularly odd to me because the the odom -> base_link transform is being published by the ekf_localization node itself! Should the lookupTransform call instead by a waitForTransform?

ayrton04 commented 10 years ago

Would you happen to have a bag file that I can use to aid in looking into this, or is the data proprietary? I used to have a call to waitForTransform there, but I found that it was causing timing issues if a given transform wasn't available. For example, if the transform required for sensor 1 isn't available yet, then waitForTransform will wait the full amount of time before it stops blocking and execution can proceed. This means that if a given transform drops out or isn't available, then the rate of measurement processing effectively drops to 1/wait_for_transform_time for all the sensors in the system. I have a few options here, one of which is to use the tf message filter (and I have been meaning to add that anyway). Another option is to first attempt to get a transform at the specified time (which is what I'm doing), catching the exception (doing this too), but then trying again with ros::Time(0) as the requested time, which will essentially just get the latest available transform.

In any case, I'll label this as a bug and look into it. Thanks!

mikepurvis commented 10 years ago

I think MessageFilter is the most correct, but even just falling back on "latest available" with a warning emitted when the age of the latest exceeds some threshold would be fine I think.

It's an unreleased/unannounced platform, but the simulator and description will be public very soon, so it's fine to share: https://www.dropbox.com/s/ubs9yqn3dd83bcd/jackal-localization.bag?dl=0

That's 10 seconds of platform encoders, imu, and the ekf node running (spewing warnings).

ayrton04 commented 10 years ago

Cool (and I say that for several reasons)! Let me look into this and get back to you.

ayrton04 commented 10 years ago

After a quick look at the raw data, I see your IMU data is in the odom frame. I have an open issue (issue #22) for fixing the way I'm handling the IMU frame. For now, if you change the frame_id of your IMU data to base_link, or give it its own "imu" frame and then make a static identity transform from base_link to imu, it will stop trying to apply its own transform.

mikepurvis commented 10 years ago

Ah, interesting. The imu/data topic is actually being produced on the PC from a data_raw topic, using imu_filter_madgwick. The imu/data topic itself is in the imu_link frame just like data_raw, which is supplied by the URDF/robot_state_publisher. I think the fix will probably be just to disable imu_filter_madgwick from publishing the odom->imu_link TF.

I'll give that a shot tomorrow and report back. Thanks very much for being willing to check this out.

ayrton04 commented 10 years ago

No problem! Just so I'm clear, you mean that the /imu/data topic is in the imu_link frame just like the imu/data_raw topic, but its message is being reported with 'odom' as its frame_id? In the bag file you provided, the header on imu/data has odom as its frame_id, and robot_localization is (currently) going to attempt to transform all IMU data into the base_link frame before using it. Apologies if that's what you were saying and I'm being dense. It's late. :)

mikepurvis commented 10 years ago

Son of a gun, you're right. That is highly unexpected behaviour from the imu filter; now investigating further.

mikepurvis commented 10 years ago

All right, the IMU filter behaviour has been corrected; all looks well now.

I'm getting an orbiting/floating platform, but I suspect that's down to a gyro bias problem with my raw data.