-Add dyn reconfigure for being able to change planning between normal and close to path mode at runtime