I am not too happy about this function. I think it should be something that gives you an array of visualization_msgs objects, which can be then published. Additionally, since this is just visualization of poses, the conversion function should be in some lower level utility package. Not sure where to put it yet. We can leave this one unfixed for the merge and figure it out later.