plot_path added. test_planners seems to be working.