Changed visualization code. Now publishing to markerarray on ros.