Path planner fixes, minor code changes