Add marker teleop

NOTE the marker should be updated better as the robot moves
(as in the interactive footsteps I did)!