how to stop the robot if something goes wrong
I am trying to setup a diagnostic mechanism on my robot so that it will stop if something goes wrong: a sensor stops working, the TF tree becomes too old, some critical node dies, etc.
I am wondering what's the best practice for that?
I am considering using the diagnostics tools. The monitors seem to be written to inform the operator only, and not take action. Maybe I could write my own node, based on robot_monitor, that would command the motors to stop whenever a diagnostic becomes ERROR...
Also, how to detect that a node died? In some cases, subscribers can report that a publisher is dead if no data was received for some time. But there are other scenarios (A node that does not publish periodically, etc.)
Finally, how to monitor the TF tree? My robot has 2 computers that tend to get out of sync (although I am using NTP to keep their clock synchronized ...) and at times this causes problems that lead the robot to the wall. How could I monitor that, besides periodically testing transforms and catching exceptions (ExtrapolationException ?) ?
For the action part, I am first going to have a node that will stop the motors if something goes wrong, but later I will include a hardware controller that will cut the power and brake if a heart beat signal stops (watchdog).
