Added test for trajectory planner