Fixed a memory leak in motion planner
WIP: Plan viewer using the engine