ENH: Implemented compatibility with PathPlanner