Time Error

[bausa]

When running the local planner (latest version), I get this error repeatedly.

[ERROR] [1451883955.408376514, 2197.280000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.240000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883955.410609732, 2197.280000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883955.606963287, 2197.480000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.440000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883955.607110702, 2197.480000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883955.809488921, 2197.680000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.640000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883955.813038204, 2197.680000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883956.007636284, 2197.880000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.840000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883956.008115028, 2197.880000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883956.209344183, 2198.080000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2189.040000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883956.209847509, 2198.080000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883956.409329772, 2198.280000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2189.240000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883956.409824265, 2198.280000000]: MSG to TF: Quaternion Not Properly Normalized````

Any ideas on what could be causing this?

[croesmann]

Thank you for sharing the issue.
I am trying to replicate the issue or even to find the cause. I've tested the latest version extensively in simulation and on a real robot without any problems (running Indigo).

In common robot configurations, the transformation between odom and map is published by some localization node (e.g. the amcl node).
There are only a few calls in the code to lookup a transformation (teb_local_planner_ros.cpp).
And there aren't any explicit calls in the initialize method (which is loaded if you start move_base with the plugin). TF lookups should take place only after you set any goal. Additionally, if any lookup is performed, the most recent transform is used (opposed to the tf lookup in your log).
Therefore this issue might occure somewhere else (e.g. during the move_base, costmap_2d or amcl initialization)?

Could you please provide me with further information:

• Are you using the latest version in the official debian repo or the latest version of this github repo (0.2.1), since they are not synchronized yet?
• Did it worked with any previous version?
• Which ROS distributation are you using (indigo/jade)?
• Does your navigation / amcl setup work with the default local planner (dwa)?
• When does this issue appear? While you are starting move_base with the teb_local_planner plugin or while you are sending any goal to the navigation stack?
• Are you running the planner on the same machine that is running the roscore (to neglect network delays)?
• Can you try to test your setup in simulation with <param name="/use_sim_time" value="true"/>

[bausa]

Are you using the latest version in the official debian repo or the latest version of this github repo (0.2.1), since they are not synchronized yet?

• I am using the v0.2.1 (build from github repo)

Did it work with any previous version?

• Yes, it was working ~ 1 month ago

Which ROS distributation are you using (indigo/jade)?

• Indigo

Does your navigation / amcl setup work with the default local planner (dwa)?

• Yes, it works with other local planners

When does this issue appear? While you are starting move_base with the teb_local_planner plugin or while you are sending any goal to the navigation stack?

• It when the robot is getting close to a goal for the first time (and the goal isn't directly next to the start position).
• I am using move_base with the teb_local_planner
• Another behavior I observed is when this issue comes up the robot oscillates over the goal.

Are you running the planner on the same machine that is running the roscore (to neglect network delays)?

• When I observed the issue it was running on two separate machines with synchronized clocks. From what I remember it works fine on 1 machine EDIT: I just tested it on one machine and I have the same issue)

Can you try to test your setup in simulation with

• The same behavior is observed when I enable sim_time.

Thanks so much for all your help. Would it be possible for you to post your test robot so I can compare the configuration and see if I can isolate the problem?

[bausa]

I was able to temporarily fix the problem by increasing the cache size of the TransformListener (defaults to 10). The issue is that the poses returned from the global plan have a time header and if too much time passes (ie the local planner encounters multiple obsticles), a transform can no longer be requested using the last step of the global plan becasue the time stored with the pose occurs prior to the last time stored in the cache. Changing this time to a larger value fixed the issue for me as long as the robot does attempt any plans with an extremely long distance (and therefore time difference).

[croesmann]

Great to hear that you have a temporary fix.

Thanks for answering my questions. They are really helpful for figuring out what's going on there.
I have a possible explanation (hopefully the right one ;-) ):

After your hint that the issue often appears close to the goal, I've checked the isGoalReached method in teb_local_planner_ros.cpp.
Here I am doing a call to TF:

    tf_->transformPose(cfg_.map_frame,global_plan_.back(),goal_pose);

This call does not wait for a new tf broadcast and tries to find a transformation related to the time stamp of the global_plan.
I think, that my global planner updates more frequently than your one such that TF is able to extrapolate. In your case, TF cannot find a pose related to that old time stamp. Since I forgot to add a correct exception handling, the quaternion check fails afterwards (your second error message).

To fix that issue (beside the exception handling), we could use the most recent TF transform, rather than the one that belongs to the time stamp of the global_plan (by changing the TF function call).
This assumption should be fine since the global plan is expected to be valid as long as the local planner is executed.

I will try to make the changes within the next few hours.

ps: You mentioned some robot oscillations at the goal: This should not occur ideally. There are three reasons that I have in mind:

1. A too small goal tolerance
2. Model mismatch: the planner thinks that the robot is able to execute the trajectory, but it can't. In that case the parameters must be adjusted (max vel / acc and/or optimization weights). I will try to spent some time on the parameter tuning during the next months. Maybe I can make it more robust for arbitrary robots (I am always happy for some feedback, if the default settings are not satisfying).
3. Before update 0.2.1 the local planner was reinitalizing each time the global planner refreshes (causing the robot to oscillate driving directions close to the goal).

[bausa]

Yes, I figured out that that line was what was causing the issue. The stamp in the last step on the global plan was expiring for me because my global planner wasn't sending the plan except when it had a new route. Increasing the frequency fixed this; however, I agree using the last known pose would be a good solution.

[croesmann]

I pushed commit 01acd28 containing a possible fix a few minutes ago. Could you please give me some feedback, if the fix works? I decided to completely avoid the TF lookup in the isGoalReached method and moved the actual checking into the computeVelocityCommand method. This also avoids redundant calls to the costmap layer.