debug info collision monitor and catch runtime exception

[fmessmer]

@ipa-mdl

exception is required for simulation as there seems to be a race condition when using sim_time from gazebo

[mathias-luedtke]

I don't think that you have to print the collisions yourself, the verbose flag should enable it internally.

[fmessmer]

verbose only prints one collision pair, while with my modifications you could see all...as well as the penetration depth...

@ipa-mdl any objections in keeping my debug info? please merge otherwise...

[mathias-luedtke]

@ipa-mdl any objections in keeping my debug info?

Yes, it should be a param, which defaults to off. And the state must be published before the debug prints. Otherwise the stop might come too late.

[mathias-luedtke]

verbose should default to false as well.

[fmessmer]

@ipa-mdl I guess I considered your remarks accordingly...