Costmap2D future robot pose with static_transform_publisher

[pavloblindnology]

When TF frames are published into the future, e.g. when using static_transform_publisher,
Costmap2D gets future stamped robot pose.
This is because it requests the latest transformation

navigation/costmap_2d/src/costmap_2d_ros.cpp

Line 579 in 3c16b37

robot_pose.header.stamp = ros::Time();

This, in turn, can lead local planner to get Extrapolation into the future error when trying to convert the latest robot pose to the frame of the plan (odom->map).

navigation/base_local_planner/src/goal_functions.cpp

Line 118 in 3c16b37

tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);

This is because the latest robot pose has been got using the latest base_link->odom transform, while odom->map may be lagging behind (e.g. if it's also published by static_transform_publisher). And previous transformation of global plan from map to odom is also lagging behind

navigation/base_local_planner/src/goal_functions.cpp

Line 113 in 3c16b37

geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(),

Proposed solution is to try to get robot pose for the current time by Costmap2D, and switch to the latest robot pose only if that is not possible.