Check number of subscribers before publishing marker

This checks whether a node has subscribed to the marker topic.
The marker is not published until someone subscribes.

Related [answers.ros.org](http://answers.ros.org/question/199299/why-do-i-need-to-enter-a-loop-to-display-a-marker/) topic.
It should be sufficient to use just `ROS_WARN` here, no need for the stream version.

Also, maybe it would be good opportunity to use the `ROS_WARN_ONCE` macro, so it doesn't continuously print this message while waiting on `rviz` to start.