Fix a glitch with the new PathPlanning

Signed-off-by: eldr4d <nikofinas@gmail.com>