Add diagnostics output

[bit-pirate]

For the turtlebot_dashboard it would be nice to have diagnostics output.

[stonier]

Why is this issue here?

[bit-pirate]

We need to check that all the required info is being published to diagnostics. Since I see this as a nice addon, I put it as TODO.