New waitForSubscriber() function that checks for first subscriber to a publisher
New generateEmptyPose() function helper
Consolidated publishing rviz messages to central publishMarker() function