Fix State-Lattice planner crashes due to FP precision loss#3531

SteveMacenski merged 2 commits intoros-navigation:mainros-navigation/navigation2:mainfrom

AlexeyMerzlyakov:sl-float-loss-fixAlexeyMerzlyakov/navigation2:sl-float-loss-fixCopy head branch name to clipboard