Let User change the PathfindingCommand skip distance

[bullzombie43]

Is your feature request related to a problem? Please describe.
We kept running into an issue when pathfinding to the reef that if we switched from targetting the left branch to the right, it would still go to the old branch position. We tracked the issue down to in PathfindingCommand it doesn't update the trajectory, if you're within 2m of the target position. This makes sense to prevent unnecessary switches as your finishing the path but led to issues as it wouldn't update the path despite that it should be targeting an entirely new pose.

Describe the solution you'd like

One simple solution would be to change the number 2.0m to a static variable skipChangeDistance, retaining the default value as 2.0m. Then providing users a static method to change this variable.

Additional context
Add any other context or screenshots about the feature request here.

I'd be happy to make this change myself, though I might need some help to build manually.