Some code cleanup in MotionPlanner