Added ability to publish robot states with color