Add time stamps to rigid body messages
Publish rigid body marker positions
Add framework to publish marker sets and other markers