Control loop missing its desired rate (waitForTransform related).

[pavloblindnology]

The PR #270 described in #196 severely slows down the control loop speed (similar problem was described in #468). The reason is that waitForTransform is called multiple times during a control loop. In dwa_local_planner it's called 3 times:

1. https://github.com/ros-planning/navigation/blob/51dfabcb4d6deee5a63329e1e6dfab6ae563d47d/dwa_local_planner/src/dwa_planner_ros.cpp#L153
2. https://github.com/ros-planning/navigation/blob/51dfabcb4d6deee5a63329e1e6dfab6ae563d47d/dwa_local_planner/src/dwa_planner_ros.cpp#L259
3. https://github.com/ros-planning/navigation/blob/51dfabcb4d6deee5a63329e1e6dfab6ae563d47d/dwa_local_planner/src/dwa_planner_ros.cpp#L274

The calls leads to either https://github.com/ros-planning/navigation/blob/51dfabcb4d6deee5a63329e1e6dfab6ae563d47d/base_local_planner/src/goal_functions.cpp#L88 or https://github.com/ros-planning/navigation/blob/51dfabcb4d6deee5a63329e1e6dfab6ae563d47d/base_local_planner/src/goal_functions.cpp#L176 both of which use 'waitForTransform' with ros::Time::now().

Calling waitForTransform with ros::Time::now() several times in a row in a single thread results in their "serialization" - each of them locks for the next TF update time step dt, till the corresponding TF transform is available again - i.e. the final lock time is ~N*dt for N runs in a row. One possible solution to this is to use single time for all waitForTransform calls, e.g. by keeping it in LocalPlannerUtil and updating it in isGoalReached method of local planner.

P.S. This issue arises mostly when using global localization package which doesn't "predict" into the future (like gmapping) - e.g. hector_slam.

[rhklite]

i saw a PR #688 for kinetic. I'm also seeing this issue described in #196 and #468 in indigo. Is this issue present in indigo aswell?

[pavloblindnology]

@rhklite Well, if you say you're seeing it in Indigo than probably it's present in Indigo.

[HappySamuel]

Hi All

Is this issue still exist in ROS Noetic?

Best, Samuel

[pavloblindnology]

Hi, I'm still on melodic, so don't know. But I presume it should still exist unless TF API in Noetic has been changed.