[ROS2] fix planning local path from single updated global path

[eliasdc]

ROS2 navigation stack has new behaviors which only update the global path based on time, distance or if it gets invalidated. This means that the path does not include new timestamps.

In transformGlobalPlan the lookup uses the timestamp of the path to get the target timestamp for the transform. This should be changed to the current time. This behavior is the same in other nav2 controllers.

To replicate the bug use any of the behaviour trees which do not update the global path, e.g. navigate_w_replanning_only_if_goal_is_updated.xml, and send a goal which takes some time to complete (+5s).

[tonynajjar]

To replicate the bug

Hi @eliasdc , I'm interested in replicating this, could you say what the bug looks like in terms of what is expected and what you see?

[tonynajjar]

I was able to replicate the bug and got.

[nice-2] [ERROR] [1688396317.770345837] [controller_server.TEBLocalPlanner]: Extrapolation Error: Lookup would require extrapolation into the past.  Requested time 4955.040000 but the earliest data is at time 4960.600000, when looking up transform from frame [map] to frame

I can also confirm that your fix avoids this lookup error.

However I don't really understand the usage of the advanced API of lookup_transform here. According to this, argument 5 should be a frame that does not change over time , yet the plan_pose.header.frame_id is used.

[ymd-stella]

@tonynajjar

I am having the same problem.

However I don't really understand the usage of the advanced API of lookup_transform here. According to this, argument 5 should be a frame that does not change over time , yet the plan_pose.header.frame_id is used.

The meaning of the original code is as follows

• Get the latest available transformation from map to map
• Get the transformation of time plan_pose.header.stamp from map to odom
• Composite the two transforms

(NOTE: plan_pose.header.frame_id is map frame. global_frame is odom frame)

The meaning of the proposed code is as follows

• Get transformation of time plan_pose.header.stamp from map to map
• Get the current time transformation from map to odom
• Composite the two transforms

The proposed code could simply be written as follows.

geometry_msgs::msg::TransformStamped plan_to_global_transform = tf_->lookupTransform(
    global_frame, plan_pose.header.frame_id,
    tf2_ros::fromMsg(clock_->now()), tf2::durationFromSec(0.5));

ref. https://github.com/ros2/geometry2/blob/2eaab2584ed6b6635a34f47df46d73bb717dee2a/tf2/src/buffer_core.cpp#L691-L709

[ymd-stella]

I noticed that logivations/teb_local_planner has a pull request. Should the same diff be applied here?