Add ability to send message back to ros. This worked out nicely and I'm pretty excited to pull it out into its own lib.