Fixed compile errors and added rviz Marker visualization publishing