MotionPlanner: add some benchmarking code