tweaked slam-traj params so now total trajectory plan time (for four waypoints) went from 1800 ms to 500 ms