Better visualization for trajectories

Changes affect mostly dwa planner. Buggy behavior still exists