Various fixes and improvements to pr2_marker_control