Trajectory is shown in RViz