MotionPlanner: reorder some code to make it easier to read
MotionPlanner: micro-optimization
MotionPlanner: remove benchmark code.
Event loop does it better.
MotionPlanner: nano-optimization
MotionPlanner: minor code beautification