Implement the diagnostics of Hero

[JosjaG]

trafficstars

The diagnostics are currently not used anywhere, so it is always assumed that all hardware is operational (which is definitely not the case). Toyota did develop a diagnostics aggregator, but if you try to use this, the robot will go into error-mode since f.e. the stereo camera's are not turned on.