Bug report

Required Info:

• Operating System: Ubuntu 20.04.4 Focal Fossa LTS
• ROS2 Version: Galactic Binaries
• Version or commit hash: ros-galactic-navigation2 1.0.12-1focal.20220526.142755
• DDS implementation: Cyclone DDS

Steps to reproduce issue

When using the REEDS-SHEPP planner from SmacHybrid plugin in conjunction with RPP, it seems that the reported distance to cusp is not actually updated to the expected value from the robot to the cusp. On inspection of the function, it looks like the distance is being computed between the cusp point which is in the map frame and the robot pose which is in the odom frame. These need not be coincident and leads to an erroneous value.

Expected behavior

Lookahead distance should get limited to cusp upon getting very close to it.

Actual behavior

If the map and odom have a large offset, the lookahead distance is never updated with the distance to cusp.

Additional information

I have fixed it by changing the function in my local implementation through frame transforms and now it works as expected. I can submit a pull request related to this issue.